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arbitrary  rigid  polyhedral  moving  object  "P"  with  three  translational  and  three 
rotational  degrees  of  freedom,  find  a  continuous,  collision-free  path  taking  "P" 
from  some  initial  configuration  to  a  desired  goal  configuration. 

^This  thesis  describes  the  first  known  implementation  of  a  complete  algorithm  (at 
a  given  resolution)  for  the  full  six  degree  of  freedom  Movers'  problem.  The  al¬ 
gorithm  transforms  the  six  degree  of  freedom  planning  problem  into  a  point  navigation 
problem  in  a  six-dimensional  configuration  space  (called  C-Space) .  The  C-Space 
obstacles,  which  characterize  the  physically  unachievable  configurations,  are 
directly  represented  by  six-dimensional  manifolds  whose  boundaries  are  five 
dimensional  C-surfaces.-x  By  characterizing  these  surfaces  and  their  intersections, 
collision-free  paths  may  be  found  by  the  closure  of  three  operators  which 

(i)  slide  along  5-di^jjraional  level  C-surfaces  parallel  to  C-Space  obstacles; 

(ii)  slide  along  1-' to  4-dlmenslonal  intersections  of  level  C-surfaces;  and 

(iii)  jump  between  6-dimensional  obstacles. 

'^Implementing  the  point  navigation  operators  requires  solving  fundamental  repre¬ 
sentational  and  algorithmic  questions:  we  will  derive  new  structural  properties 
of  the  C-Space  constraints  and  show  how  to  construct  and  represent  C-surfaces 
and  their  intersection  manifolds definition  and  new  theoretical  results 
are  presented  for  a  slx-dlmensionalNC-Space  extension  of  the  generalized 
Voronol  diagram,  called  the  "'^-Voronol  dlagrarf'*^ whose  structure  we  relate  to 
the  C-surface  intersection  manifolds.  NThe  representations  and  algorithms  we 
develop  Impact  many  geometric  planning  Moblems,  and  extend  to  Cartesian 
manipulators  with  six  degrees  of  freedom.\ 
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Motion  Planning  with  Six  Degrees  of  Freedom 


Bruce  Randall  Donald 

Abstract:  The  motion  planning  problem  is  of  central  importance  to  the  fields 
of  robotics,  spatial  planning,  and  automated  design.  In  robotics  we  are  interested 
in  the  automatic  synthesis  of  robot  motions,  given  high-level  specifications  of 
tasks  and  geometric  models  of  the  robot  and  obstacles.  The  Mover’s  problem 
is  to  find  a  continuous,  collision-free  path  for  a  moving  object  through  an 
environment  containing  obstacles.  We  present  an  implemented  algorithm  for  the 
classical  formulation  of  the  three-dimensional  Movers’  problem:  Given  an  arbitrary 
rigid  polyhedral  moving  object  P  with  three  translational  and  three  rotational 
degrees  of  freedom,  find  a  continuous,  collision-free  path  taking  P  from  some  initial 
configuration  to  a  desired  goal  configuration. 

This  thesis  describes  the  first  known  implementation  of  a  complete  algorithm 
(at  a  given  resolution)  for  the  full  six  degree  of  freedom  Movers’  problem.  The 
algorithm  transforms  the  six  degree  of  freedom  planning  problem  into  a  point 
navigation  problem  in  a  six-dimensional  configuration  space  (called  C-Space).  The 
C-Space  obstacles,  which  characterize  the  physically  unachievable  configurations, 
are  directly  represented  by  six-dimensional  manifolds  whose  boundaries  are  five 
dimensional  C-surfaces.  By  characterizing  these  surfaces  and  their  intersections, 
collision-free  paths  may  be  found  by  the  closure  of  three  operators  which  (i) 
slide  along  5-dimcnsional  level  C-surfaces  parallel  to  C-Space  obstacles;  (ii)  slide 
along  1-  to  4-dimcnsional  intersections  of  level  C-surfaces;  and  (iii)  jump  between 
6-dimcnsional  obstacles. 

Implementing  the  point  navigation  operators  requires  solving  fundamental 

representational  and  algorithmic  questions:  we  will  derive  new  structural  properties 

of  the  C-Space  constraints  and  show  how  to  construct  and  represent  C-surfaces  and 

their  intersection  manifolds.  A  definition  and  new  theoretical  results  arc  presented 

for  a  six-dimensional  C-Space  extension  of  the  generalized  Voronoi  diagram,  called 

the  C-  Voronoi  diagram,  whose  structure  we  relate  to  the  C-surfacc  intersection 

manifolds.  The  representations  and  algorithms  we  develop  impact  many  geometric 

planning  problems,  and  extend  to  Cartesian  manipulators  with  six  degrees  of 

freedom.  - - - - - 
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Geometric  Planning  Problems 

Introduction  and  Statement  of  the  Problem 

The  motion  planning  problem  is  of  central  importance  to  the  fields  of  robotics, 
spatial  planning,  and  automated  design.  In  robotics  we  arc  interested  in  the 
automatic  .synthesis  of  robot  motions,  given  high-level  specifications  of  tasks  and 
geometric  models  of  the  robot  and  obstacles.  The  problem  is  to  find  a  continuous, 
collision-free  path  for  a  moving  object  through  an  environment  containing  obstacles; 
hence  it  has  also  been  called  the  Find-Path  or  Piano  Movers'  problem.  In  its  most 
general  formulation  the  object  can  have  an  arbitrary  number  of  hinges  and  joints, 
and  in  some  cases  coordinated  motion  planning  for  multiple  objects  has  been 
considered.  We  will  coniine  ourselves  to  the  classical  formulation  of  the  Movers’ 
problem:  Given  an  arbitrary  rigid  polyhedral  moving  object  P,  find  a  continuous, 
collision-free  path  taking  P  from  some  initial  configuration  to  a  desired  goal 
configuration.  We  are  particularly  interested  in  the  3-dimcnsional  Movers’  problem, 
for  an  object  with  3  translational  and  3  rotational  degrees  of  freedom.  This  thesis 
describes  the  first  known  implementation  of  a  complete  algorithm  (at  a  given 
resolution)  for  the  full  G  degree  of  freedom  Movers’  problem. 

I.I.  What  arc  Geometric  Planning  I’roblcms? 

Our  work  has  impact  on  a  class  of  geometric  planning  problems.  In  robotics 
we  arc  typically  interested  in  motion  planning  for  a  mobile  robot  or  manipluator. 
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FiRurc  1.1.  All  ox.'itnpic  of  a  solution  path  for  the  classical  Mover’s  problem  with  six  degrees 
of  freedom.  This  illustration  is  a  “time-lapse”  picture  of  a  path  found  by  our  planner  for  a 
haniincr-shapcd  object.  In  all  our  examples,  the  workspace  is  bounded  by  a  box  (which  is  not 
shown).  This  solution  path  requires  use  of  all  three  rotational  degrees  of  freedom. 

In  Computer-Aided  Design  (CAD),  the  problem  of  automated  structural  design 
for  n  structural  members  is  also  an  instance  of  the  most  general  form  of  the 
Mover’s  problem.  The  problem  of  determining  whether  an  object  can  be  assembled 
as  designed,  and  of  generating  an  assembly  plan  if  it  can,  is  also  in  this  class. 
Examples  of  geometric  planning  problems  include: 

(i)  The  Find- Path  OT  Movers 'problem  is  to  find  continuous,  collision-free  path 
for  one  or  more  moving  objects  in  the  presence  of  obstacles.  Find-path  problems 
fall  into  two  broad  categories:  single-body  and  multiple-body  motion  planning 
problems.  In  the  classical  Movers’  problem,  a  single  rigid  polyhedral  object  must 
be  moved  through  a  workspace  containing  polyhedral  obstacles.  For  the  linked 
or  hinged  body  Movers’  problem,  a  set  of  moving  objects  connected  via  joints  or 
linkages  must  be  moved.  An  industrial  robot  arm  is  a  typical  example  of  such  an 
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object.  In  the  coordinated  planning  problem,  a  number  of  independent  (i.c.,  not 
necessarily  linked)  objects  must  be  moved.  An  algorithm  for  multiple  body  motion 
planning  must  ensure  that  the  moving  objects  collide  neither  with  the  walls  nor 
with  each  other. 

(ii)  The  find-space  problem  is  to  Cnd  a  collision-free  placement  for  one  or 
more  objects  in  a  field  of  obstacles.  By  analogy  with  the  find-path  problem,  we 
can  speak  of  the  classical,  linked- body,  and  coordinated  find-space  problems.  In 
computer-aided  design  and  automated  design,  the  find-space  problem  is  typically 
subject  to  additional  geometric  constraints.  Lozano- Perez  (1983)  grouped  find-path 
and  find-space  algorithms  together  as  the  spatial  planning  problems. 


(iii)  The  fine-motion  problem  entails  motion-planning  along  obstacle  surfaces, 
typically  while  maintaining  some  applied  force.  Collision-free  paths  and  placements 
avoid  obstacles:  however,  for  many  tasks  in  robotics  and  in  automated  design,  it 


Figure  1.3.  Application  Example:  Pl:inning  for  an  industrial  robot  arm  with  six  dcgrtxis  of 
freedotn  is  an  example  of  the  linked-body  movers’  problem.  (Figure  courtesy  of  Rodney  Brooks). 

is  necessary  to  plan  motions  and  placements  in  contact  with  obstacle  surfaces.  For 
example,  consider  the  tasks  of  welding,  insertion,  and  assembly  in  robotics.  These 
tasks  require  compliant  motions,  entailing  consideration  of  additional  physical 
constraints  such  as  friction,  kinematics,  and  force  control.  However,  the  compliant 
motion  planning  problem  has  a  strong  geometric  flavor  and  its  solution  requires 
the  tools  of  spatial  planning  (see  Mason  (1981),  Erdmann  (1984)). 

(iv)  Recently,  researchers  have  begun  to  consider  motion  planning  with 
uncertainty  (Mason  (1981),  Brooks  (1982),  Lozano-Perez,  Mason,  and  Taylor  (1983), 
Erdmann  (1984)).  Broadly  speaking,  uncertainty  may  arise  from  inaccuracy  in 
object  models,  sensors,  or  control.  Motion  planning  with  uncertainty  also  presumes 
algorithms  and  representations  from  spatial  planning. 

As  we  can  see,  all  geometric  planning  problems  contain  components  of  the 
spatial  planning  problem,  especially  if  the  underlying  geometries  are  the  same.  In 
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FiK'irc  I  •4-  fixariiple  from  compuler-nidcd  design;  AutoniAtically  generated  llat-platc  structure 
from  Donald  (1983b).  How  can  we  generate  structural  patterns  subject  to  the  constraints  of  the 
building  envelope  and  mechanical  core? 

particular,  for  high-dimensional  configuration  spaces,  the  theoretical  analyses  of 
Mason  (1981),  Lozano-Perez,  Mason,  and  Taylor  (1983),  and  Erdmann  (1984)  all 
presume  geometric  results  which  are  derived  in  this  thesis. 

This  work  impacts  all  geometric  planning  problems.  To  illustrate  the  theoretical 
results,  we  address  one  particular  problem,  namely  the  classical  Movers’  problem 
with  six  degrees  of  freedom.  Our  algorithms  immediately  generalize  to  applications 
involving  gross-motion  and  fine-motion  planning  for  Cartesian  manipulators  with 
six  degrees  of  freedom. 


[30] 


Figure  1.21.  A  detail  of  the  palli  for  the  I.-Shapod  example.  The  detail  is  in  "time-lapse"  format, 
and  shows  a  complex  double  rotation  near  the  goal  configuration. 

Examples  of  “classicar’  find-path  problems  solved  by  our  planner  may  be  found 
throughout  this  chapter,  and  also  at  the  end  of  chapter  2  (section  2.4,  " Examples 
of  the  Local  Experts  in  Use").  See  fig.  1.5-21,  1.22-28,  and  2.7-21.  In  general, 
geometric  planning  problems  with  more  than  three  degrees  of  freedom  have  proven 
extremely  difficult  to  solve.  We  believe  that  in  part,  this  difficulty  has  been  due 
to  the  unresolved  issues  in  the  mathematics  of  spatial  planning.  By  solving  these 
problems  for  the  six  degree  of  freedom  case,  and  illustrating  the  results  for  the 
find-path  problem  (which  holds  considerable  intrinsic  interest),  we  hope  to  provide 
a  geometric  foundation  which  will  make  all  geometric  planning  problems  feasible. 
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KiRiirc  1.22.  (3  Views  of  the  “Putile  Problem").  In  Ihi.s  Find-path  problem,  the  L-«h:iped  object 
mil'll  be  moved  uroiiiid  the  diagonally- pl.-iced  obstacle.  Here  the  t<-shapcd  object  is  shown  in  the 
initial  and  goal  coiiFigiirations. 

1.2.  A  Simple  Example:  IIow  to  Find  a  Path  for  a  Point  Amidst  3-D 
Polyhedral  Obstacles 

We  will  begin  by  discussing  an  algorithm  for  navigating  a  single  point  amidst 
polyhedral  obstacles  in  three-dimension  Euclidean  space  9?'^.  We  then  review  the 
conGguration  space  transformation  of  Lozano-Perez  (1983),  which  transforms  the 
problem  of  reasoning  about  the  motion  of  a  polyhedral  object  to  reasoning  about  a 
single  point  in  configuration  space.  If  the  configuration  spare  is  isomorphic  to  3?^, 
then  the  point  navigation  algorithm  can  be  applied  directly  to  find  collision  free 
paths.  In  this  thesis  we  will  generalize  the  point  navigation  algorithm  to  work  in 
the  configuration  space  for  a  three  dimensional  polyhedral  object  with  six  degrees 
of  freedom. 

The  six  degree  of  freedom  planner  is  based  on  the  following  analogy:  suppose 
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we  wish  to  find  a  path  for  a  point  in  3?‘*,  avoiding  collisions  with  polyhedral 
obstacles,  where  each  polyhedron  is  modeled  as  the  intersection  of  a  finite  number 
of  half-spaces  of  9?'^  One  solution  might  be  to  move  until  the  point  comes  in 
contact  with  a  polyhedron,  and  then  to  move  around  the  obstacle  by  traversing  the 
edge-graph  on  its  boundary.  (Refer  to  figure  1.39,  ignoring  the  caption  for  r.ow). 
lOach  arc  in  tlu'  edge-graph  is  the  intersection  of  two  surfaces  bounding  half-spaces. 
I'^vcn  if  the  polyhedra  are  allowed  to  overlap,  the  technique  will  still  work  since  their 
intersections  have  the  same  structure.  Naturally,  we  will  also  need  some  technique 
for  jumping  from  one  obstacle  to  another. 

To  summarize,  we  can  find  a  collision-free  path  for  a  point  amidst  obstacle 
polyhedra  in  9?'  through  the  closure  of  three  operators: 

The  I’oint  Navigation  Operators 

Operator  (i)  slides  along  the  l-diinrnsionn}  edge.s,  ivhich  lie  in  tlic  intersections 

of  the  obsLaede  planes; 

Operator  (ii)  slides  along  tlie  two  dimensional  obstacle  planes,  which  contain 

faces  of  the  obstacle  polyhedra; 

Operator  (iii)  jumps  from  one  3-dimensional  obstacle  to  another. 

Wc  now  review  the  configuration  space  transformation.  Using  this  transfor¬ 
mation  in  its  sirrqilest  form,  tlie  find-path  problem  in  three  dimensions  without 
rotations  is  reduced  to  the  point  navigation  problem  amidst  polyhedral  obstacles. 

1.3.  Configuration  Space 

The  configurnliun  of  a  moving  object  is  a  vector  of  parameters  representing 
its  combined  translation  and  orientation,  relative  to  a  specified  coordinate  system. 
For  the  classical  Movers’  problem  in  the  plane,  a  typical  configuration 


represents  a  displaceincnt  (translation)  of  {x,y),  and  a  rotation  by  0.  (For  example, 
imagine  a  polygon  displaced  by  {x,y),  and  rotated  by  0  about  one  of  its  vertices). 
For  tlie  six  degree  of  freedom  classical  Movers’  problem,  a  typical  configuration 


X  =  {x,y,z,R{0)) 

represents  a  displacement  (translation)  of  {x,y,z),  and  a  three  dimensional  rotation 
.?(0).  The  tlirec  dimensional  rotation  group  is  a  three  parameter  family;  typical 
representations  of  rotations  include  h'uler  angles,  (Symon  (1071)),  spherical  angles, 
and  (juaternions  (Hamilton  (1060)).  For  example,  if  the  fader  angles  0  = 
arc  employed,  then  they  determine  a  3  by  3  rotation  matrix  which  functions  as 
,?(0)  in  the  rotation  group.  It  is  convenient  to  identify  the  rotation  operator  with 
its  parameterization,  that  is,  to  express  X  as 

X  {x,y,z,ip,0,4>). 

Using  configuration  space,  reasoning  about  the  motion  of  a  complicated  three- 
dimensional  body  amongst  obstacles  may  be  transformed  into  reasoning  about 
a  point  in  a  six  dimensional  configuration  space.  The  transformation  described 
by  Lozano- Perez  (H)S3)  entails  “shrinking”  the  moving  object  to  a  point,  and 
correspondingly  “growing”  the  obstacles.  In  principle,  the  point  may  then  be 
navigated  around  the  grown  obstacles  by  means  of  the  point  navigation  operators 
(above). 

In  this  thesis,  the  point  navigation  operators  will  be  generalized  to  the 
six  dimensional  conliguration  space  of  the  classical  Movers’  problem. 

In  order  to  present  onr  algorithm  for  planning  in  C-Spnee,  it  is  necessary  to 
review  the  basics.  We  [)resent  an  introduction  to  representations  in  configuration 
siiace  at  two  levels:  first,  we  present  an  intuitive  discussion.  Next,  we  present  a 
more  delaifi'd,  slightly  more  mathcmal.ically-oricntcd  exposition.  For  the  sake  of 
ri'adability,  there  is  some  redundancy  in  the  sections.  Those  who  arc  encountering 
configurat ion  space  for  the  first  time  may  wish  to  postpone  reading  the  latter 


section  for  now. 


Figure  1.29.  These  ligurca  show  an  obstacle  polygon  D  and  a  moving  polygon  A.  A  must  be 
moved  around  fi  to  the  goal  configuration. 

1.3.1.  Rciircscnlations  in  Configuration  Space;  An  Intuitive  Treatment 

Figure  1.29  is  an  example  of  the  classical  Movers’  problem  in  two  dimensions, 
without  rotations.  A  is  a  moving  object  which  must  be  moved  from  the  start 
configuration  to  the  goal  configuration,  around  an  obstacle  polygon  B.  The  start 
and  goal  conGgurations  may  be  expressed  as  two  dimensional  vectors  of  the  form 
(i,  1/)  which  represent  the  displacement  of  a  vertex  vi  on  A  from  a  fixed  coordinate 
frame.  The  displacement  is  a  rigid  translation  of  the  polygon  A.  The  C-Space  of 
this  Movers’  problem  is  the  space  of  two  dimensional  translations,  which  is  the 
same  as  the  Cartesian  plane.  Lozano-Perez  (1983)  demonstrated  a  transformation 
which  shrinks  A  to  the  vertex  uj,  while  inversely  growing  B.  The  grown  obstacle  for 
i?  is  a  C-Space  obstacle  called  CO{B),  and  is  shown  in  figure  1.31.  (We  will  discuss 
the  details  of  this  transformation  later).  The  problem  of  moving  the  polygon  A 
from  the  start  to  the  goal  is  transformed  into  the  problem  of  navigating  the  point 


vi  around  the  C-Space  obstacle  shown  in  figure  1.31. 


Both  A  and  B  are  convex;  non-convex  objects  arc  represented  by  overlapping 
unions  of  convex  polyhedra.  The  C-Space  obstacle  CO[B)  is  constructed  by 
considering  all  feasible  interactions  of  the  edges  and  vertices  of  A  and  B.  Each  such 
interaction  generates  a  constraint  which  is  manifest  as  an  edge  of  CO{B).  We  say 
that  an  interaction  between  a  vertex  of  A  and  an  edge  of  B,  or  between  an  edge  of 
A  and  a  vertex  of  B,  is  feasible  if  there  is  some  pure  translation  which  can  bring 
the  vertex  and  edge  in  contact  without  causing  A  and  B  to  overlap.  For  example, 
the  set  of  all  possible  interactions  of  A  and  B  is  the  union  of  the  two  cartesian 
products 


{ei,C2,C3}  X  {v[,V2,V3,v!i  } 


and 


.18 


However,  at  the  depicted  orientation  of  A,  only  these  interactions  are  feasible: 

{  (vi,  e{),  (ui ,  €2).  (ei .  ^3).  (’^2, 4)>  {^2,  V4).  (^^3,  {es,  v[)  }. 

It  is  easy  to  visualize  the  translation  that  will  bring  any  of  these  pairs  into  contact. 
Furthermore,  note  that  (for  example),  no  translation  can  bring  V3  in  contact  with 

ei- 

Now  imagine  that  A  is  allowed  to  rotate  about  vi-  At  different  orientations, 
there  will  be  different  sets  of  feasible  contacts.  We  say  that  the  constraints  associated 
with  feasible  contacts  arc  applicable  constraints.  It  should  be  clear  that  at  any  given 


orientation,  only  certain  constraints  will  be  applicable.  The  orientations  for  which  a 
given  constraint  is  applicable  form  its  applicability  region.  The  applicability  regions 
for  each  constraint  in  this  problem  arc  angular  sectors  of  the  form  [0i  ^  0  ^  ^u]- 
This  simply  means  that  there  exists  a  range  of  angles  in  which  a  particular  contact 
is  feasible.  This  range  of  angles  may  be  geometrically  visualized  as  a  sector  of  a 
circle. 

When  A  is  allowed  to  rotate,  the  geometry  of  CO{D)  changes  as  B  varies,  and 
as  the  set  of  applicable  constraints  changes.  As  the  edges  and  vertices  of  A  rotate 
about  ri,  the  constraints  they  generate  sweep  out  ruled  surfaces  which  bound 
CO{n)  in  a  three  dimensional  C-Space.  (The  C-Space  is  three  dimensional,  since 
A  now  has  three  degrees  of  freedom:  i,  y,  and  0).  At  any  fixed  orientation  Oq,  an 
x-y  slice  of  CO{I3)  is  a  polygon,  called  slice{CO{D),Oti).  Figure  1.31  shows  such  a 
slice  at  the  depicted  orientation  of  A.  With  each  edge  of  slice[CO[B),0)  there  is  an 
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associated  half-plane  containing  slice[CO{D),0),  whose  boundary  contains  the  edge 
(see  figure  1.32).  The  intersection  of  these  half-planes  is  exactly  sUce(CO(B),  0). 
As  0  changes,  different  half-planes  are  used  to  construct  sltce(CO(B),  0).  I3y  (1) 
deriving  the  line  equation  of  the  boundary  of  these  half-planes  in  terms  of  the 
orientation  0,  and  (2)  by  determining  the  applicability  region  for  each  half-plane 
constraint,  we  can  characterize  slices  of  the  C-Space  obstacle  slice{CO{B),0)  as  0 
changes.  Thus  we  can  characterize  the  three  dimensional  C-Space  obstacle  CO{B). 
This  representation  may  be  used  to  develop  planning  algorithms  for  the  Movers’ 
problem  with  two  translational  and  one  rotational  degrees  of  freedom  (sec  Drooks 
and  Lozano- Perez  (1983)). 

In  this  thesis  we  develop  such  a  representation  for  the  six  degree  of  freedom 
Movers’  problem.  There  are  several  problems  which  must  be  solved.  Because 
the  structure  of  the  rotation  group  is  more  complicated  in  three  dimensions, 
the  applicability  regions  for  constraints  in  a  six  degree  of  freedom  C-Space  are 
geometrically  much  more  complicated.  While  in  two  dimension^  the  appli''ability 
regions  may  be  visualized  as  sectors  on  a  circle,  in  three  dimensions  they  are 
complicated  three  dimensional  manifolds  on  the  projective  3-sphcrc.  It  is  important 
to  characterize  these  regions,  since  they  specify  where  a  constraint  is  applicable. 
We  will  discuss  some  of  the  other  problems  presently. 

Generalizing  the  Point  Navigation  Operators  Requires  Solving 
Representational  Questions 

In  order  to  generalize  the  point  navigation  operators  to  the  C-Space  of  the 
classical  Movers’  problem,  we  must  be  able  to  characterize  the  surfaces  of  C-Space 
obstacles,  and  the  intersections  of  these  surfaces.  The  first  two  operators,  then,  must 
slide  along  the  C-surfaces  and  their  intersections.  In  the  next  section,  we  discuss  some 
of  the  representational  issues  involved  in  developing  such  operators.  For  example, 
when  rotations  are  allowed,  the  C-surfaces  are  curved.  In  the  six-dimonsional  space 
of  the  classical  Movers’  problem,  each  C-surface  is  a  five-dimension.il  submanifold  of 
C-Space,  and  the  intersection  of  two  such  surfaces  is  a  four-dimensional  manifold. 
Thus  it  is  possible  to  slide  along  such  an  intersection  with  four  degrees  of  freedom. 


1.3.2.  RcprcHcntations  in  Connguraliuii  Space:  A  More  Formal  Treatment 

In  this  section,  we  present  a  somewhat  more  abstract  formulation  of 
representational  issues  in  C-Space.  Some  readers  may  wish  to  postpone  reading  this 
section  until  later.  We  will  proceed  :\s  follows:  first,  we  will  outline  an  important 
representational  question  which  must  be  solved  in  this  thesis.  Next,  we  discuss 
how  to  represent  volumes  (such  as  C-Space  obstacles)  in  C-Space.  In  the  course 
of  this  discussion,  several  terms  will  be  definerl  in  context  by  means  of  intuitive 
descriptions.  At  the  end  of  this  section,  under  the  heading  Working  Definitions,  we 
will  summarize  and  formalize  the  definitions  to  the  extent  that  will  be  required  in 
chapters  1  and  2. 

The  Domain  Question 

Until  now,  geometric  planning  problems  with  more  than  three  degrees  of 
freedom  have  proved  resistant  to  solution.'  In  this  thesis  we  provide  such  an 
algorithm  for  find-path  with  six  degrees  of  freedom  (the  classical  Movers’  problem). 
'I'lie  resistance  of  these  problems  has  largely  been  due  to  unresolved  mathematical 
issues  and  questions  relating  to  the  structure  of  configuration  space  and  to  the  nature 
of  C-Space  constraints,  (although  for  fine-motion  and  planning  with  uncertainty 
there  arc  of  course  additional  issues). 

One  fundamental  theoretical  problem  for  high-dimensional  configuration  spaces 

may  bo  stated  as  follows:  in  a  configuration  space  C  with  rotations,  each  C-Space 

obstacle  may  be  represented  by  the  intersection  of  a  finite  number  of  half-spaces. 

Each  half-space,  in  turn,  is  defined  by  a  real-valued  C-function  on  C-Space.  For 

example,  the  half-space  might  be  the  set  of  configurations  where  the  C-function 

is  nog.'itive.  However,  each  C-function  is  a  partial  function  on  C,  whose  domain 

is  a  complicated  region  in  C-Spacc.  This  greatly  complicates  the  representation 

for  C-Space  obstacles  and  C-surfaces  (see  figure  1.36).  Moreover,  until  now  the 

domains  of  the  C- functions  were  unknown  for  all  but  the  one-dimensional  rotation 

group.  One  of  our  first  tasks  will  be  to  derive  the  domains  of  all  C-functions  for 

the  classical  Movers’  problem  with  six  degrees  of  freedom. 

'  llowcvi  r,  pri'vioii.s  work  lias  provided  an  cxisl.riire  proof  of  a  polynomial  time  al('oritliin  for 
(crlain  spalial  planniiu'  protilcms.  In  addition,  there  are  approxiinale  alj'orillmis  for  .some  of  llu'se 
prohleins.  .See  oiir  rr'view  of  previous  work. 
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There  are  several  related  problems,  for  which  we  also  present  solutions.  This 
allows  us  to  construct  a  complete  geometric  representation  for  the  configuration  space 
of  the  classical  Movers’  problem  with  six  degrees  of  freedom.  This  representation 
impacts  all  the  geometric  planning  problems  we  have  discussed,  and  extends 
naturally  to  Cartesian  Manipulators. 

Representing  Volumes  in  Configuration  Space 

The  dimensionality  of  configuration  space  is  the  number  of  degrees  of  freedom 
in  the  parameter  space,  i.c.,  the  number  of  degrees  of  freedom  available  to 
the  moving  object(s).  Thus  the  classical  Mover’s  problem  in  the  plane  has  two 
translational  and  one  rotational  degrees  of  freedom,  while  in  three  dimensions  it 
has  three  translational  and  three  rotational  degrees  of  freedom.  The  configuration 
spaces  for  these  problems  arc  three  and  six  dimensional  manifolds,  respectively. 
As  the  number  of  degrees  of  freedom  increases,  a  geometric  planning  problem 
becomes  harder.  There  are  several  reasons  for  this.  First  of  all,  when  rotations  arc 
allowed,  configuration  space  ceases  to  be  Euclidean,  and  the  C-surfaces  become 
curved.  Furthermore,  the  non-commutativity  and  multiple-connectedness  of  the 
three-dimensional  rotation  group  are  classical  difficult  issues  in  mathematics.  In 
addition,  it  can  be  shown  that  the  computational  complexity  of  spatial  planning 
grows  exponentially  with  the  dimensionality  of  the  C-Space. 

A  fundamental  issue  for  geometric  planning  algorithms  is:  how  should  C-Space 
obstacles  and  surfaces  be  represented  and  computed? 

A  volume  in  a  configuration  space  C  may  be  represented  by  the  intersection  of 
a  finite  number  of  half-spaces  (see  figure  1.33).  Each  half-space  may  be  defined  via 
some  smooth,  real-valued  function  function  on  C, 

/,  :  C  ^  3?. 

For  example,  (fig.  1.34)  suppose  /,(i,j/)  =  ax  +  by  -t  c,  for  some  constants  a,  b, 
and  c.  The  kernel  of  /  is  the  line  where  f{x,y)  =  0.  The  halfspacc  /if  is  the 
portion  of  the  plane  where  f{x,y)  is  negative.  C-functions  such  as  /,  arise  in  the 
two  dimensional  Movers’  problem,  at  a  fixed  orientation. 


- r -  -I  »  y  f  --m  » 

In  general,  the  (closed)  half-space  hj"  is  the  set  of  all  points  in  C  where  /,•  is 
negative-valued  or  zero: 


hi  =  {u  G  C|  /,{«)  <  0}. 

The  common  intersection  of  a  number  of  such  half-spaces  can  yield  a  volume  in 
C.  Lozano-Perez  (1983)  showed  how  C-Space  obstacles  can  be  modeled  in  this 
manner,  and  gave  the  form  of  the  functions  /,.  Note  that  each  C-surface  lies 
within  the  kernel  of  some  constraint  /,.  Fine-motion  strategics  and  algorithms  for 
planning  with  uncertainty  need  to  compute  the  normals  and  tangent  spaces  to 
these  C-surfaccs.  The  normal  can  usually  be  derived  from  the  gradient  V/,  (this 
requires  placing  an  appropriate  Ricmannian  metric  on  the  tangent  space).  When 
a  real-valued  function  /,•  on  configuration  space  is  used  to  describe  constraints  in 


that  C-Space,  (i.e.,  C-Space  obstacles),  we  call  it  a  C-function.  The  form  and 
interpretation  of  C-functions  are  presented  later. 


Fipure  1.35.  IlliiMt.r.'xIion  of  thr  cl.'ifl.siral  Movrr’s  problem  in  three  dirnenitions.  H  is  an  obstacle, 
anil  A  is  an  obji’cl  wliirli  iiinsl  be  rnovi-d  around  li.  /!,,«,  shows  A  in  the  start  configuration, 
and  shows  A  in  the  desired  (;oal  ronfiRiiralioii.  C  is  the  i>olyhedron  whirh  is  the  C-Space 

obstacle  from  II  for  A  at  orientation  B|.  At  orientation  i)',  the  (l-Sjxirv  obstacle  from  //  is  a 
(lilfi  rent  polyhedron,  which  we  show  as  C'. 


Example:  (See  Ogure  1.35).  Consider  the  Movers’  problem  for  a  three  dimensional 
polyhedron  A  which  can  translate  but  not  rotate  amidst  polyhedral  obstacles.  The 
configuration  space  for  A  is  a  three-dimensional  vector  space  of  translations,  which 
it  is  convenient  to  identify  with  3?’*.  Each  constraint  /j  will  be  linear  on  'Ji'*,  and 
the  kernel  of  /v  is  a  plane.  Each  such  plane  bounds  a  C-Space  obstacle  (such  as  C 
or  C  in  figure  1.35).  The  C-Space  obstacles  are  possibly  overlapping  polyhedra  in 
9J'*.  The  find-path  problem  in  the  transformed  space  is  that  of  navigating  a  point 
past  the  union  of  these  C-Space  polyhedra  (see  figure  1.39). 

In  this  formulation  there  is  a  fundamental  underlying  assumption;  /,  is  a  total 
function  on  C,  that  is,  the  domain  of  /,  is  all  of  C-  In  the  example  this  is  not  a 
problem,  since  the  domain  of  each  linear  C-function  is  the  entire  space.  A  function 
whose  domain  is  a  subset  of  C  is  called  a  partial  function  on  C.  When  rotations  are 
allowed,  C-functions  become  partial  functions. 
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Why  do  C-Spare  constraints  become  partial  functions  when  rotations  are 
introduced’  Consider  the  classical  find-path  problem  in  two  dimensions,  for  a 
moving  polygon  A  which  can  translate  and  rotate  in  the  plane  (see  figure  1,37).  A 
configuration  of  A  may  be  represented  by  three  parameters,  {x,y,0).  The  surfaces  of 
the  C-Space  obstacle  for  B  arise  from  each  of  the  feasible  contacts  (or  interactions) 
between  the  edges  and  vertices  of  A  and  /?.  Thus  the  constraint  functions  (which 
we  have  been  calling  /,)  are  defined  by  considering  pairwise  interactions  of  edges 
and  vertices  of  A  with  vertices  and  edges  of  B.  Every  such  pair  such  as  e,,  and  vj 
will  generate  a  smooth,  real-valued  C-function  on  configuration  space. ^  Each 

constraint  is  designed  such  that  their  conjunction  enforces  a  disjointness  criterion 
for  A  and  B.  However,  not  all  interactions  are  possible  at  any  given  orientation. 
For  example,  at  the  depicted  orientation  0]  of  A,  edge  can  interact  with  vertex 
V,  but  edge  e\  cannot:  .at  orientation  0],  no  translation  can  bring  e\  in  contact 


M'or  ttx.'  form  of  Llic  (J-fiiiirl ioiin,  sov  cloipU-rs  3  and  4. 


with  V  while  maintaining  the  disjointness  of  the  interiors  of  A  and  B.  We  say  the 
associated  C-fnnetion  is  not  applicable  at  orientation  Oi.  In  other  words,  no 

configuration 


is  in  the  domain  of  fc,,v  I'^Jich  constraint  fr„,Vk  is  apiilicablc  only  at  certain 
orientations,  and  hence  each  can  be  considered  a  partial  function  on  the  C-Space.^ 

In  three  dimensions  (see  figures  1.35  and  1.39),  the  surfaces  of  the  C -Space 
obstacle  for  B  arise  from  each  of  the  feasible  contacts  between  the  vertices,  edges, 
and  faces  of  A  and  B.  By  analogy  with  figure  1.37,  it  is  clear  that  not  all  of  these 
interactions  are  feasible  at  any  given  orientation.  Thus  the  (^fllnrtions  describing 
C-surfaces  for  spatial  planning  with  six  degrees  of  freedom  must  also  bo  partial 
functions. 

Working  Defiritions:  Review  and  Summary 

We  now  summarize  and  formalize  the  key  definitions  ami  concepts  required  in 
this  chapter  and  the  next: 

Configuration  Space:  (Formal  definition)  Configuration  space  is  the  product 
space  of  the  space  of  translations  and  the  space  of  rotations  for  an  object.  In  three 
dimensions,  the  space  of  translations  is  Kuclidean  3-spacc  and  the  space  of 
rotations  is  the  3-dimcnsional  rotation  group  or  Special  Orthogonal  Group,  50(3). 
50(3)  is  isomorphic,  to  the  intersection  of  the  Special  Linear  Croup  (the  set  of 
all  real  3  X  3  matrices  with  determinant  1)  and  the  Orthogonal  Group  (which 
may  be  thought  of  as  the  set  of  matrices  with  orthonormal  rows  and  lolumns). 
The  orthonormality  of  rows  and  of  columns  are  equivalent  conditions.  50(3)  is 
isomorphic  to  ,  the  3-sphere  5*  with  opposite  points  idenlifit'd.  [B'^  is  also  known 
as  tlic  projective  S-nphere).  .9'  is  isomorphic  the  group  of  unit  ciuaternions.  For  the 
classical  .Movi'rs’  [irobh'in  we  will  employ  configuration  sjiace,  IJ?’  X  50(3).  We  will 
denote  the  classical  Movers’  Problem  with  three  translational  d  three  rotational 

llrdok-i  l.oz.ino  Ikrez  (l!)S3)  for  .'i  clisriissioti  of  (.lie  doriiai  ii.s  of  ( '  fii  ml  ions  for  file  two 
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degrees  of  freedom  as  &DOF.  In  practice,  we  will  represent  rotations  as  members 
of  a  three-parameter  family  (for  example,  Euler  Angles),  but  we  must  keep  in  mind 
that  they  parameterize  an  isometry  and  that  5I?*  X  50(3)  is  not  a  vector  space. 
If  the  Euler  angles  [ijj,0,4>)  arc  employed  to  represent  the  orientation  of  a  rigid 
polyhedral  body,  a  typical  configuration  X  in  3?'*  X  50(3)  has  the  form 

X  =  {x,y,z,ip,0,<i>). 

We  will  sometimes  adopt  the  notation 

x  =  (r,e), 

where  r  denotes  a  three-dimensional  translation  vector,  and  0  some  three 
dimensional  rotation.  This  second  notation  is  independent  of  the  particular 
representation  chosen  for  rotations;  the  first  isn’t.  If  Euler  angles  are  employed,  we 
may  think  of  0  as  the  “vector”  of  Euler  angles, 

C-Space  Obstacle:  (Informal  definition)  Configuration  space  obstacles  are 
(possibly  overlapping)  six  dimensional  manifolds  (with  boundary)  which  correspond 
to  sets  of  configurations  that  would  cause  collisions  of  the  moving  object  with  real 
space  obstacles. 

Free  space:  The  free  space  is  that  subset  of  C-Space  which  lies  within  no 
C-Space  obstacle.  3’he  free  space  will  be  denoted  F. 

Applicability  Sri:  (Informal  definition)  Jtefer  to  figures  1.29-32  and  1.33-38,  and 
recall  that  C-Spacc.  obstacles  are  represented  by  the  intersection  of  a  finite  number 
of  half-spaces.  (To  be  formal  we  should  call  them  half  hypcrspaces].  The  boundary 
of  each  half-space  is  a  C-surface,  and  contains  a  boundary  patch  of  the  C-Space 
obstacle.  lOach  C-surface  5  may  be  expressed  as  the  kernel  of  a  real-valued  function 
/  on  C-Space.  The  C-funclion  f  is  negative  on  the  obstacle  side  of  the  half-space 
C-Space  obstacle,  and  positive  on  the  other  half.  In  the  literature  C-functions  have 
been  called  constraints,  since  they  express  constraints  on  the  possible  motions  for 


an  object.  A  biirface  parallel  to  S  is  called  a  level  C-siirface,  and  represents  the  set 
of  configurations  where  /  has  a  certain  fixed  value.  This  value  is  termed  the  level 
of  the  level  C-surface.  The  boundary  of  the  C-Space  obstacle  is  a  special  case  of 
level  C-surface,  where  the  level  is  zero.  We  have  seen  that  at  any  given  orientation, 
only  certain  C-functions  (and  their  associated  C-surfaces)  arc  applicable.  This  is 
because  only  certain  contacts  arc  f(  asiblc  between  the  faces,  edges,  and  vertices 
which  generate  the  C-functions.  We  call  this  set  of  C-functions  the  applicability  set. 
For  example,  in  1  29-32,  at  the  depicted  orientation  the  applicability  set  is 


{  (vi,  eO,  (ut ,  4)>  (ei ,  W;,),  (v2, 63),  (e-i,  W4),  (v3,  e[),  (cs,  v^)  }. 

(Actually,  the  applicability  set  is  the  set  of  C-functions  generated  by  these 
vertex/edge  and  edge/vertex  pairs,  but  since  there  is  a  one-to-one  correspondence 
between  the  generator  pairs  and  the  C-functions,  we  can  write  it  this  way).  In  later 
chapters,  we  will  demonstrate  algorithms  for  computing  the  applicability  set,  and 
for  docornpo.sing  rotation  space  into  regions  where  the  applicability  set  is  invariant. 

Redundant  and  Non-redundant  Constraints:  (Informal  definition).  If  a  config¬ 
uration  X  is  in  free  space,  the  set  of  constraints  which  is  (locally)  relevant  to  motion 
planning  from  X  is  a  subset  of  the  applicable,  positive-valued  C-functions  at  X. 
However,  the  value  of  a  C-function  does  more  than  merely  indicate  which  side  of  a 
C-snrface  X  is  on.  A  C-function’s  value  represents  the  translational  distance  to  that 
surface.  Thus,  C-functions  provide  a  collection  of  pseudo-metrics  on  C-Space.  Using 
these  metrics,  it  is  possible  to  order  C-surfaces  by  their  closeness  to  a  configuration 
X  (simply  sort  the  C-fuiictions  on  their  value  at  X).  We  say  that  a  C-surface  is 
redundant  if  it  is  subsumed  by  a  nearer,  intervening  constraint.  In  figure  1.38,  for 
example,  /  and  g  arc  non- redundant  constraints  at  X,  but  h  is  redundant  since  it 
is  subsumed  by  /.  It  is  useful  to  determine  the  set  of  non-redundant  constraints  at 
A'  since  this  is  the  sm.illest  set  of  constraints  that  arc  locally  relevant  to  motion 
planning.  We  provide  a  formal  definition  of  redundancy  in  chapter  3. 

Robot,  Moving  Object,  and  Piano:  All  of  these  terms  have  been  employed  in 
the  literature  to  refer  to  the  moving  object  for  which  a  collision-free  path  must 
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Figure  1.38.  h  ia  a.  redundant  cofistraint. 

be  found.  Our  moving  object  i.s  modeled  as  the  possibly  overlapping  union  of  a 
Gnite  set  of  convex  polyhedra.  The  union  is  rigid,  but  not  necessarily  convex  or 
connected.  The  moving  object  has  three  translational  and  three  rotation  degrees  of 
freedom.  To  avoid  monotony,  we  may  employ  the  term  robot  to  refer  to  the  moving 
object.  The  terminology  is  justiCed  in  part  by  the  fact  that  our  algorithm  extends 
straightforwardly  to  Cartesian  manipulators. 


I.  Conipntational  Theory 

J^aths  can  be  found  in  C-Space  by  the  closure  of  three  operators: 

(i)  slides  along  I-  to  '1-diniensionaI  intersections  of  level  C-surfaces; 

(ii)  slitles  along  5-diinensional  level  C-surfaces; 

(iii)  jumps  between  G-dimensional  obstacles. 


n.  licprcHcntatioii  and  Algorithm 
Search  Algorithm  Employing  the  Three  Operators 

t 

Solve  the  Intersection  Problems. 

Develop  a  represention  for  the  intersection  manifolds. 

tr 

Solve  open  questions  about  the  structure  of  6DOF  constraints.  Derive  and 
represent  structur.al  properties  of  tlic  constraints,  for  example,  the  doniiiins  of 
defining  partial  functions.  Develop  decomposition  algorithms. 


[II.  Implementation 
Implement  the  6DOF  planner. 


IV.  New  I'heoretic.al  llesults  _ _ _ _ 

'I'he  structure  of  6DOF  constraints:  Theorems  on  the  domains  and  domain 
topology  of  the  defining  partial  functions. 

Theorems  on  the  applicability  decomposition. 

'['he  C-Voronoi  Diagram  (CVD). 

The  Equivalence  Theorem  for  intersection  manifolds  and  the  CVD. 

Criteria  for  designing/integrating  local  and  global  planning  algorithms. 


1.3.3.  Generalizing  the  Point  Navigation  Operators 


Consider  a  three-dimensional  configuration  space  containing  smooth,  curved 
C-Space  obstacles.  Observe  that  the  point  navigation  operators  will  work  even  if 
the  surfaces  are  curved  and  complicated,  as  long  as  we  can  find  their  intersections. 
For  the  two-dimensional  Mover’s  problem  (for  a  polygon  allowed  to  rotate  and 
translate  in  the  plane),  we  employ  a  configuration  space  X  5'.  51?"^  is  the  space  of 
two  dimensional  translations,  and  5'  is  the  unit  circle,  on  which  one-dimensional 
rotations  may  be  represented.  C-functions  arc  of  the  form  /,  :  X  5’  -+  3?  and 

are  valid  within  some  sector  Ai  of  5*.  A  C-surface  is  the  set  of  configurations 
where  /,  is  zero.  Although  these  C-functions  are  complicated  expressions  containing 
trigonometric  terms  of  the  form  xcosO  and  ysinO,  it  is  possible  to  solve  two 
such  C-surfaces  simultaneously  to  obtain  an  intersection  curve  in  3?^  X  5*  which 
is  parametric  in  0  (these  intersection  manifolds  are  derived  in  chapter  4).  The 
analogy  to  navigating  a  point  through  a  polyhedral  environment  should  now  become 
clear:  the  faces  of  the  polyhedra  correspond  to  C-surfaccs  in  3?^  X  5’  and  the 
edge-graphs  to  the  graph  of  C-surfacc  intersections.  By  searching  the  graph  of 
C-surfacc  intersections  we  can  find  a  path  in  configuration  space,  if  one  exists. 

Planning  in  a  Six  Dimensional  C-Space 

Our  planner  for  a  six  dimensional  C-Space  is  based  on  the  idea  of  moving  along 
the  intersections  of  level  C-surfaces  in  free  space,  parallel  to  the  boundaries  of  C- 
Space  obstacles.  In  the  example  above,  the  coincidence  between  the  dimensionality 
of  configuration  space  and  Euclidean  sp.ace  was  serendipitous;  edges  on  polyhedra 
corresponded  to  curves  in  3?^  X  S\  and  faces  to  2-dimensional  surfaces.  However,  in 
a  six  dimensional  C-Space,  the  C-surfaces  are  5-dimensional  and  their  intersections 
are  4-dimcnsional  sub-manifolds.  Intuitively  this  means  that  the  set  of  possible 
motions  while  complying  with  two  constraints  is  a  four-parameter  family. 

Our  idea  is  as  follows;  Suppose  we  could  slide  along  C-surfaces  (see  figure  1.39). 
In  addition,  suppose  we  could  intersect  C-surfaccs  to  construct  a  lower  dimensional 
manifold  in  C-Space  which  contained  paths  along  (or  around)  the  boundary  of 
C-Space  obstacles.  By  sliding  along  C-surfaces,  and  by  sliding  along  the  intersection 
of  C-surfaces,  we  should  be  able  to  devise  an  algorithm  which  can  circumnavigate 


Figure  We  CHP  represent  tlie  configuration  of  a  polyhedron  /I  by  a  pair,  (T*,  0),  where  T  is 

a  translation  of  A  and  0  is  a  rotation  of  A.  The  prohleni  of  moving  A  from  configuration  (r,,0i) 
(’■(.^2)  is  iraiisforineci  to  the  problem  of  navigating  a  coiijiguration  point,  r,  past  C,  which 
is  tlie  C-Spacc  obstacle  due  to  It.  Si  and  S2  are  C-surfaccs  bounding  C.  The  configurations  c,- 
lie  on  the  boundary  of  C,  while  di  is  in  free-spare.  Two  trajectories  around  li  arc  shown.  Note 
that  the  path  segments  (c6,(r,,0^))  and  (d i , (r^,  02))  must  also  include  a  rotation.  (The  actual 
reference  [Kiint  is  at  the  centroid  of  A,  but  for  the  purposes  of  exposition,  we  have  placed  it  at  a 
vertex  as  shown). 

C-Space  obstacles.  (Of  course,  we  also  need  a  way  to  plan  motions  which  “jump” 
from  one  obstacle  to  another). 

Example:  Figure  1.39  shows  how  such  a  planning  algorithm  might  work.  The 
planner  moves  through  free-spacc  from  (fj,©!)  until  it  strikes  a  C-surface  Si  at 
Cl  -  From  C|  a  path  is  found  towards  C2  sliding  along  the  C-surfacc  Si-  We  say  the 
planner  slides  along  Sj  to  rj.  Configuration  lies  on  an  intersection  manifold  of 
the  C-surfaces  Sj  and  S>.  The  path  segment  (c2,C3)  slides  along  this  intersection 
manifold,  which  lies  on  the  boundary  of  C.  A  path  (02,03,04,05,01;)  is  planned  along 
the  graph  of  intersection  manifolds  on  the  boundary  of  C.  From  05  we  leave  the 
boundary  of  C,  and  translate  and  rotate  through  frec-space  to  (>'„,02).  This  path 


is  an  idealized  example  of  planning  along  C-surfaces  and  intersection  manifolds  of 
C-surfaces.  The  implemented  planner  finds  a  path  similar  to  ((ra,  0i),  di ,  (r^,,  Oj)) 
(see  chapter  2).  The  path  segment  ((ra,0),  di)  is  parallel  to  the  C-surface  5i,  and 
we  say  that  it  slides  along  a  level  C-surface  for  S\.  The  path  segment  (d|,(rj,0)) 
is  along  a  level  C-surface  for  S-j.  These  level  C-surfaces  intersect  along  a  manifold 
in  free-space  containing  d\  (imagine  extending  the  faces  Si  and  S2  beyond  the 
boundary  of  C  until  they  intersect). 

We  will  derive  the  necessary  mathematical  theory  and  tools  relating  to  C- 
surfaces  and  their  intersection  manifolds,  and  present  algorithms  for  moving  and 
planning  paths  in  C-Space.  Some  of  the  issues  we  will  address  include: 

(i)  What  is  an  appropriate  representation  for  constraints  in  a  six  dimensional 
C- Space? 

(ii)  How  do  we  plan  motions  using  constraints  whose  domains  change  with  the 
motions? 

(iii)  How  can  trajectories  in  C-Space  be  intersected  with  C-surfaccs  whose  domains 
change  along  the  trajectory? 

(iv)  How  can  intersection  manifolds  be  constructed  in  C-Space? 

(v)  How  are  motions  planned  which  slide  along  C-surfaces  and  intersection  manifolds? 

It  is  useful  to  develop  a  terminology  for  evaluating  algorithms  and  repre¬ 
sentations  for  geometric  planning  problems.  An  algorithm  employing  an  approximate 
representation  does  not  characterize  the  constraints  exactly.  A  complete  algorithm 
(for  a  given  resolution)  is  guaranteed  to  find  a  solution  if  one  exists  (at  that 
resolution).  In  general,  construction  of  a  complete  algorithm  mandates  the  employ¬ 
ment  of  a  complete  representation.  A  brute-force  algorithm  tries  to  find  a  solution 
through  exhaustive  search.  Heuristic  algorithms  fall  into  two  (overlapping)  classes: 
heuristically  complete,  and  heuristically  fast.  See  also  the  review  of  previous  work 
(below). 

The  implemented  algorithm  we  present  for  the  classical  Mover’s  problem  with 
six  degrees  of  freedom  employs  a  complete  representation  of  the  configuration  space 
constraints,  and  a  complete  search  algorithm  (for  a  given  resolution). 


1.4.  Local  versus  Global 
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Local  algorithms  for  the  find-path  problem  examine  local  constraints  in  some 
neighborhood  of  real  space  or  in  C-Space,  and  propose  motions  based  on  the 
geometry  of  the  neighborhood.  Typically,  local  algorithms  are  implemented  as 
searches,  and  the  examination  of  constraints  near  a  search  node  leads  to  the 
selection  and  application  of  some  local  operator  to  move  the  robot  in  space.  For 
example,  in  our  algorithm  for  the  six  degree  of  freedom  Movers’  problem,  the  local 
constraints  correspond  to  the  geometric  structure  of  C-surfaces  in  a  neighborhood, 
and  local  operators  consist  of  motions  along  C-surfaces.’  In  general,  a  local  planning 
algorithm  will  be  complete  if  (l)  the  closure  of  the  local  operators  is  comi)lete  for  the 
arcwise-connected  components  of  C-Space,  and  (2)  each  local  operator  attempted 
ensures  that  a  collision-free  path  exists  between  configurations  in  the  search. 

However,  our  observation  has  been  that  in  general,  even  complete  local 
algorithms  can  get  lost  examining  irrelevant  local  constraints.  In  particular,  without 
.idequate  knowledge  of  the  connectivity  of  a  workspace  and  the  classes  of  paths 
it  contains,  such  methods  may  choose  impossible  or  ill-advised  candidate  paths; 
hence  they  may  take  a  long  time  to  converge. 

A  global  find-path  algorithm  attempts  to  construct  a  model  of  the  connectivity 
of  the  workspace.  Wo  believe  that  the  connectivity  of  configuration  space  can  be 
inferred  from  the  connectivity  of  real  space.  Good  hypotheses  about  the  channels, 
or  clas.^cs  of  paths  through  free-space  can  serve  as  guidance  for  a  more  detailed 
method.  While  there  exist  several  proposals  for  global  approaches  to  the  Movers’ 
problem,  in  Donald  (H)83a)  we  attempt  to  formalize  criteria  for  the  design  of  such 
algorithms.  A  global  planner  based  on  these  criteria  was  implemented,  and  coupled 
with  a  complete  local  algorithm  to  form  an  integrated  planning  system. 

(Channels  arc  an  encoding  of  free-space  corresponding  to  the  classes  of  paths 

within  an  environment.  An  implementation  exploiting  this  global  model  of  the 

connectivity  of  free-space  has  been  able  to  solve  two  dimensional  find-path  problems 

in  several  minutes  which  formerly  took  many  hours.  The  algorithm  is  essentially 

'’I’liis  cxatiiple  i.'i  illusl  rrilive  :iri<l  typical  of  the  h)ral  roiiatraints  and  operators.  The  impleiiieiitcd 
planner  is  more  coinf)liral<'d,  as  we  shall  see. 
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a  problem-solving  strategy  using  a  homcoinorphic  reduction  of  the  search  space. 
See  Donald  (1983a)  for  a  description  of  the  channel  algorithm.  In  appendix  III, 
we  discuss  the  design  and  integration  of  local  and  global  planning  algorithms  in 
3?'*  X  50(3). 

1.5.  Review  of  Previous  Work 

1.5.1.  Introduction 

In  this  section  we  review  previous  work  on  geometric  planning  problems. 
We  also  give  a  formal  characterization  of  completeness  for  the  spatial  planning 
problems.  A  survey  of  robotics  issues  in  robot  motion  planning  can  be  found 
in  Brady,  ct  al.  (1980).  For  related  work  on  the  mover’s  problem,  see  Brooks 
(1983a),  Lozano-Perez  (1981,  1983),  Lozano-Perez  and  Wesley  (1979),  Brooks  and 
Lozano- Perez  (1983),  Schwartz  and  Sharir  (1982a),  Rcif  (1979),  Moravec  (1979), 
Udupa  (1977)  and  Ilopcroft  and  Wilfong  (1984).  Wingharn  (1977)  and  Popplcstone, 
Ambler,  .and  Bellos  (1980)  consider  related  issues  in  geometric  planning  problems. 
Some  issues  in  automated  structural  design  arc  addressed  in  Donald  (1983b).  For 
a  review  of  geometric  modeling  techniques,  see  Baer,  ICastman,  et  al.  (1979)  and 
Rcquicha  (1980). 

1.5.2.  Coinplcxity-Theorctic  Results 

In  seminal  work  on  the  complexity  of  the  Movers’  problem,  Reif  (1979)  has 
shown  that  the  motion  planning  problem  for  a  robot  with  an  arbitrary  number 
of  degrees  of  freedom  in  the  form  of  arm-like  linkages  is  P-Space-hard.  ilopcroft, 
Joseph,  and  Whitesides  (1982)  have  shown  similar  results  for  planar  manipulators 
with  n  linkages.  In  general  it  has  been  found  that  with  n  degrees  of  freedom,  the 
problem  is  P-Space-\\a.rd.  Happily,  Schwartz  and  Sharir  (1982a)  h.ave  demonstrated 
the  existence  of  a  polynomial- time  algorithm  for  the  Movers’  problem  with  fixed 
degrees  of  freedom,  where  the  size  of  the  problem  is  measured  in  the  number  of 
ob.stacle  faces  in  the  environment.  The  algorithm  of  Schwartz  and  Sharir  (1982a) 
for  the  classical  Movers’  problem  is  unfortunately  of  time  complexity 


whore  n  is  polynotiiially  dependent  on  tlie  number  of  faces  in  the  environment,  and  d 
is  the  number  of  degrees  of  freedom.  For  b  degrees  of  freedom  this  becomes 
Hence  it  serves  chiefly  as  an  existence  proof  for  a  polynomial-time  algorithm. 

The  coordinated  motion  problem  has  also  been  given  some  attention:  Schwartz 
and  Sharir  (1982b)  address  the  problem  for  2  and  3  circular  bodies  moving  amidst 
j)olygonal  obstacles  in  the  plane.  'I'he  coordinated  motion  system  has  degrees  of 
freedom  ecjual  to  the  sum  of  the  degrees  of  freedom  of  the  moving  bodies.  These 
results  lead  us  to  expect  ex{)onential  behavior  from  all  motion- planning  algorithms 
as  the  number  of  degrees  of  freedom  grows.  For  these  reasons  we  will  confine 
ourselves  to  the  classical  Movers’  problem,  which  has  6  degrees  of  freedom  in 
3-dimcnsional  space. 

Reif  (1979)  also  sketches  a  polynomial-time  algorithm  for  the  classical  Movers’ 
problem,  but  it  appears  incomplete  in  that  it  ignores  constraints  arising  from  the 
interactions  of  faces  of  the  moving  object  with  vertices  of  obstacles,  and  does  not 
consider  edge-edge  interactions  in  3  dimensions. 

1.5.3.  Work  In  Computational  Geometry  and  Robotics 

The  foundations  of  our  approach  lie  in  Lozano-Perez  (1981 ,  1983),  Lozano-Perez 
and  Wesley  (1979)  and  Schwartz  and  Sharir  (1982a).  The  problem  of  moving  a 
complex  polyhedral  object  among  obstacles  is  transformed  to  the  problem  of  finding 
a  path  for  a  j)oint  in  a  high-dimensional  configuration  space. 

Hrnoks  and  Lozano-Perez  (198.3)  have  implemented  a  general  path- finding 
algorithm  for  a  polygonal  object  in  the  plane  with  two  translational  and  one 
rotational  degrees  of  freedom.  Their  planner  uses  hierarchical  subdivision  of  the 
.3  (limeii^ional  configuration  space  X  S’’.  The  subdivision  algorithm  has  been 
specialized  to  the  particular  geometry  of  the  Movers’  problem  in  X  S’’  and 
while  in  princi[de  it  is  extensible  to  the  6  <legree  of  freedom  [iroblem,  its  space- 
complexity  in  high  dimensions  is  likely  to  be  unattractive.  A  problem  with  the 
hierarchical  subdivision  strategy  is  that  it  has  trouble  exidoiling  coherence  in 
C-Sjiar.c.  Its  spatial  taxonomy  is  restricted  to  filled,  empty,  and  mixed,  in  a 
world  where  almost  everything  is  mixed.  Mixed  cells  are  subdivided  until  an  empty 


region  is  found.  However,  it  is  hard  to  propagate  this  useful  information  to  guide 
the  search  through  nearby,  unrefined  cells  in  the  subdivision.  One  goal  of  the 
algorithms  and  representations  in  this  thesis  has  been  to  exploit  coherence  in  the 
configuration  space.  The  intuitive  appeal  is  that  the  intersections  of  C-surfaces  “go 
somewhere  useful”  (i.e.,  around  the  obstacles).  We  will  adopt  an  approach  which 
exploits  the  coherence  of  C-Space  obstacles  by  moving  along  the  intersections  of 
high-dimensional  C-manifjlds“  parallel  to  the  obstacle  boundaries. 

Lozano-Porej!  (1981)  has  described  approximate  solutions  for  Cartesian  manip¬ 
ulators  with  6  degrees  of  freedom  (in  principle)  which  consider  3-dimensional 
slice-projections  of  Configuration  space.  In  practice  these  approximations  are  only 
reasonably  accurate  for  Cartesian  manipulators  with  4  degrees  of  freedom.  In 
principle,  the  C-Space.  constraints  on  motion  defined  by  Lozano-Perez  (1983)  can 
be  extended  directly  to  a  6  degree  of  freedom  planner;  indeed,  this  is  our  starting 
point.  However  there  are  many  interesting  and  complex  problems  to  work  out  (see 
B.ro'^ks  (J983b)  for  another  discn.s.sio.n  of  these  problems).  In  particul.ar,  there  are 
many  unresolved  mathematical  details  for  the  6  degree  of  freedom  case.  Given 
the  mathematical  model,  there  still  remains  the  issue  of  a  complete  planner  which 
exploits  the  model. 

1.5.4.  Global  Methods 

Global  methods  for  path  planning  attempt  to  construct  a  model  of  the 
connectivity  of  froe-space  which  can  be  related  to  tlie  Voronoi  diagram  (see 
Drysdale  (1983)).  In  particular.  Brooks  (1983a)  has  implemented  a  2-dimonsional 
path-planner  which  models  the  frcc-spacc  as  an  overlapping  union  of  Generalized 
Cones  (Binford  (1971)).  Each  cone  provides  orientation  constraints  on  motion  within 
the  cone,  and  these  constraints  are  intersected  to  find  a  translational  path  along  the 
cone  axes  (called  spines)  interspersed  with  rotations  at  the  spine  intersections.  This 
work  was  extended  to  a  six-link  manipulator  for  moving  payloads  with  1  degrees 
of  freedom  (Brooks  (1983b)).  The  extended  algorithm  uses  the  same  cone  model, 
but  sweeps  each  cone  vertically  to  build  prisms  at  horizontal  slices  through  the 
workspace.  This  method  works  well  when  the  payload  (or  polygon)  is  small  and 


(I-iti.uiifold  i.s  :i  manifold  in  a  conlij'iiration  space. 


convex  in  a  relatively  uncluttered  obstacle  environment.  It  is  not  at  all  clear  how 
to  extend  the  algorithm  to  large,  non-convex  moving  objects,  or  how  to  consider 
more  than  one  rotational  degree  of  free<Iom  at  a  time.  Nevertheless  the  concept  of 
computing  “freeways,”  or  “channels”  through  free-space  is  attractive  in  that  it  can 
[)rovide  global  guidance  to  local  algorithms  (such  ;is  C-Spac.e  methods),  and  can 
enumerate  good  hypotheses  about  candidate  paths  through  complex  workspaces. 

Using  an  approach  called  retraction,  O’Dunlaing  and  Yap  (1982),  O’Dilnlaing, 
Sharir  and  Yap  (1982)  const  ruct  a  Voronoi  diagram  for  a  two-dimensional  workspace 
and  consider  moving  simple  objects  (a  disc,  a  line-segment)  along  it.  I'his  technique 
was  ment  ioned  by  Brooks  (1983a).  It  has  not  yet  been  extended  to  {)olygonal  objects 
or  3-dimcnsional  cases.  We  will  address  this  issue  by  considering  Generalized 
Voronoi  Manifolds. 

1.5.5.  Approximation  and  Completeness 

Planning  problems  have  two  components:  characterizing  the  constraints,  and 
searching  for  a  solution  which  satisfies  the  constraints.  One  attempts  to  achieve  a 
complete  (in  some  sense,  “exact”)  characterization  of  the  constraints,  and  a  complete 
search  algorithm  for  the  representation.  Since  the  Mover’s  problem  is  a  continuous 
nrathernatical  decision  problem,  we  must  in  general  consider  a  discrcltzed  version  of 
the  problem  (see  Reif  (1979)),  for  example,  we  might  represent  the  polyhedral  input 
models  as  systems  of  linear  inc<iualities  within  a  fixed  accuracy  e,  with  0  <  c  <  1. 
In  fact,  there  are  two  kinds  of  resolution  limit.  Any  algorithm  which  employs  real 
arithmetic  has  a  resolution  limited  to  the  machine  precision.  (Schwartz  and  Sharir 
(1982a)  cm{)loy  rational  and  algebraic  numbers  instead).  I'or  the  find-path  problem, 
we  are  interested  exclusivt'ly  in  the  physically  realizable  paths,  that  is,  those  paths 
lying  entirely  within  open  sets  of  free  s{)acc.  The  resolution  limit  Reif  mentions  is 
essentially  a  bound  on  how  small  an  open  set  can  become  before  it  is  no  longer 
considered  op<'n.  'Phe  open  set  resolution  limit  is  ty{)ically  greater  than  the  machine 
precision. 

Almost  all  (ind-path  search  algorithms  arc  complete  only  to  this  fixed  resolution; 
the  notable  exception  is  Schwartz  and  Sharir  (1982a),  which  appears  to  be 
search-complete  and  resolution  independent.  We  should  stress  that  for  a  com[)lctc 


representation,  the  resolution-dependence  is  in  practice  not  a  scv'ere  restriction. 
However,  the  effect  of  a  complete  search  algorithm  running  on  an  approximate 
characterization  of  the  constraints  is  not  clear.  In  principle,  in  case  of  search  failure, 
it  is  sometimes  possible  to  refine  the  approximation  and  redo  the  search  until  a 
path  is  found.  This  possibility  has  rarely  been  exploited  however,  and  introduces  a 
number  of  unpleasant  technical  and  conceptual  issues.  A  complete  search  running 
on  an  approximate  representation  will  in  general  result  in  an  incomplete  algorithm. 
For  these  reasons  we  would  prefer  a  complete  characterization  of  the  constraints 
coupled  with  a  complete  search  algorithm. 

We  will  place  this  thesis  in  the  context  of  previous  work  by  considering  the 
following  criteria; 

(i)  For  what  degrees  of  freedom  does  the  algorithm  apply? 

(ii)  Is  the  representation  (the  characterization  of  the  constraints)  complete? 

(iii)  Is  the  search  complete  (at  a  given  resolution)? 

(iv)  lias  the  algorithm  been  implemented? 

Approximate  Representations 

Much  of  previous  work  has  focused  on  approximate  characterizations  of  the 
constraints.  Approximate  representations  may  (1)  artificially  restrict  the  degrees  of 
freedom  in  a  problem,  (2)  bound  objects  in  real-space  by  simple  objects  such  as 
spheres,  or  prisms  with  parallel  axes,  while  considering  some  subset  of  the  available 
degrees  of  freedom,  (3)  discretize  configuration  space  at  certain  orientations,  or  (4) 
ajjproxirnate  swept  volumes  for  objects  over  a  range  of  orientations.  Such  restricted 
planning  systems  may  lose  solutions  which  require  exploiting  all  six  degrees  of 
freedom.  An  approximation  of  the  obstacle  environment,  robot  model,  or  C-Space 
obstacles  can  result  in  a  transformed  find-path  problem  which  has  no  solution. 

Some  approximate  algorithms -for  example,  tho.se  of  Brooks-run  quite  fast 
for  the  class  of  problems  that  they  address.  In  general,  speed  has  been  a 
motivating  factor  in  the  design  of  these  approaches.  We  also  observe  that  some 
approximate  methods  were  motivated  by  the  clifTiculties  of  modeling  constraints  in 
a  full  ()-dimcnsional  C-Spnr.ti.  These  diflicultios  in  turn  stemmed  from  unresolved 
mathematical  problems  relating  to  both  ('-Space  it.self  and  to  the  structure  of 


C-Space  constraints.  However,  even  with  a  complete  mathematical  model  in  hand, 
wo  arc  still  confronted  with  the  problem  of  devising  a  complete  planner  which 
works  using  the  full  set  of  constraints. 

I'he  coiifigu ration  space  of  the  three  dimensional  classical  Movers’  [)roblcm  with 
six  tlcgrcos  of  frf'odom  is  S?'"*  X  SO(li),  where  ..90(3)  denotes  the  thrta'-dirnensional 
rotation  group.  In  this  thesis  we  first  complete  tlie  mathematical  framework  for  the 
configuration  space  9^'*  X  .90(3)  and  present  solutions  for  some  heretofore  unsolved 
pro’  'runs,  'fhis  foundation  then  allows  us  to  propose  and  construct  a  complete 
planner  exploiting  the  full  set  of  constraints  .and  (>  degrees  of  freedom  for  motion 
planning  in  9?'*  X  .90(3). 

In  this  section,  we  characterize  the  completeness  of  previous  work.  Utdess  noted, 
search-completeness  is  resolution-dependent.  Schwartz  and  Sharir  (l9S2a)  describe 
(;om])letv>  repr(\sentat ions  and  co(n()lctc  (unimplernented)  search  algorithms  for  2D 
.arid  3D.  d’hese  theoretical  algorithms  appear  to  be  resolution-independent.  Brooks 
and  I.ozano-Perez  (1983)  doscrii)e  complere  representations  and  search  algorithms 
for  tht!  [iroblem  in  9?“  X  5’.  Lozano-Perez  (1981,  1983),  hozano-Perez  and  Wesley 
(1979)  give  approximate  representations  (except  for  translation)  with  complete 
search  algorithms  for  9?“  X  .S'  nnd  9?'^  X  .90(3).  These  ai)proximate  representations 
also  model  Cartesian  manipulat.ors.  .Most  algorithms  for  9?'*  X  .90(3)  can  bo  extended 
for  Cartesian  manipidators  in  a  similar  manner.  For  translations,  Lozano-Perez’ 
algorithms  are  f:om()letc  to  the  macliine  n'solution.  Brooks  (1983;i)  provides  an 
ap[iroximate  constr.iint  char.icterizat ion  with  .a  complete'  search  algorithm  for 
9C’  A  Brooks  (1983b)  extends  I, his  for  a  linked  arm  carrying  a  payload  with 
degrees  of  freedom  9?'*  X.9'.  A  significant  contribution  of  Bre)oks  was  the  .uldressing 
of  the  issue  of  jointed  arms,  d'hi!  o[)en  set  re.solution  limit  for  the  Voronoi  methods 
(for  sim|)le  objects  in  two  dimensions)  is  no  larger  than  the  imichine  iirecision. 
Pdiipa  (1977)  and  W'iddoes  (1971)  used  approximate  representations  and  incomplete 
search  algorithms  in  addressing,  Iind-|>ath  for  jointed  arms. 

In  this  light,  we  r-an  char.icterize  our  algorithm  as  follows: 

•  7’/n,s  Ihesm  presents,  the  Jirat  implemented,  reprrsenlntiori  coinplcle,  search- 

rotnpletr.  algorilinn  (at  a  (jieen  resolution )  h>r  the  elnssieal  Movers’  problem  in 


X  S0(3). 


l.G.  An  Outline  of  this  Thesis:  Research  Contributions 

In  this  thesis  we  present  a  local  algorithm  for  the  six  degree  of  freedom 
classical  Movers’  problem.  The  channel  based  algorithm  developed  in  Donald  (1984) 
is  described  in  Donald  (1983a). 

At  the  heart  of  this  research  lie  certain  mathematical  developments  that  may 
seem  fairly  abstract  at  first  reading.  To  motivate  the  mathematics,  we  first  present, 
in  chapter  2,  the  design  and  implementation  of  a  six  degree  of  freedom  planning 
system  for  the  classical  Movers’  problem.  The  description  of  the  planning  algorithm 
assumes  that  certain  representations  and  mathematical  tools  are  available.  In 
subsequent  chapters,  we  develop  these  tools  in  answer  to  the  following  questions, 
for  which  chapter  2  assumes  solutions: 


Representational  and  Algorithmic  Questions 


(i)  What  is  an  appropriate  representation  for  constraints  in  a  six  dimensional 
C-Space?  (Chapter  3). 

(ii)  In  the  six  dimensional  C-Space  of  the  classical  Movers’  problem,  the  domain  of 
each  constraint  is  the  product  space  of  3?'*  and  a  complicated  three-dimensional 
manifold  (with  boutulary)  on  the  projective  three-sphere.  What  are  these  regions, 
and  what  is  their  structure?  What  representation  can  be  used  for  these  domains? 
(Chapter  3). 

(iii)  How  do  we  plan  motions  using  constraints  whose  domains  change  with  the 
motions?  (Cliaptcr  5). 

(iv)  Given  a  trajectory  in  C-Space,  it  is  necessary  to  find  where  it  intersects 
the  boundary  of  C-Space  obstacles.  How  can  trajectories  be  intersected  with 
C-surfaces  whose  (h'lnains  change  along  the  trajectory?  (Chapter  1,  5). 

(v)  How  can  intersection  manifolds  be  constructed  in  C-Space?  (Cliapter  4). 

(vi)  How  are  motions  planned  that  slide  along  C-surfaces  and  intersection  manifolds? 
(Chapter  4,  2). 

(vii)  How  can  rotation  s{)ace  be  decomposed  into  regions  where  the  set  of  applicable 
constraints  is  invariant?  ((’hapter  5). 


How  to  Read  this  Thesis 


Chapter  2  —covering  tlie  design  and  implementation  of  the  search  algorithm  in 
C-Space — presents  the  most  heuristic  component  of  this  research.  It  is  also  in  some 
sense  the  most  accessible  chapter  to  the  non-specialist.  However,  do  not  confuse 
chapter  2’s  iinjilemcntation  details  and  search  heuristics  with  the  representational 
and  algorithmic  framework  developed  under  the  considerably  more  formal  .egis  of 
chapters  3  through  6.  The  thesis  is  structured  so  that  those  jirefering  a  presentation 
more  in  keeping  with  the  traditional  style  of  mathematical  exposition  may  read  the 
chapters  on  GDOF  planning  in  the  alternative  order: 

(l)  Geometric  Planning  Problems 

(3)  Questions  of  Representation:  C-functions  and  Applicability  Constraints  in  a 
Six  Dimensional  Configuration  Space 

(4)  Mathematical  Tools  for  Motion  Planning  in  a  Six  Dimensional  Configuration 
Space 

(5)  Moving  Through  Rotation  Space 

(G)  The  C-Voronoi  Diagram  and  its  Relationship  to  Intersection  Manifolds 

(^)  A  Planning  System  for  the  Classical  Movers’  Problem  with  Six  Degrees  of 
Freedom. 

In  the  alternative  order,  the  representations  and  algorithms  are  derived  and 
presented  first,  and  the  ajijilication  and  implemenl.ition  is  presented  last. 

Chapter  3  presents  a  formal  framework  in  which  several  open  questions  about 
configuration  space  constraints— -notably  (ii)  (above)  -  m:iy  be  solved.  Chapter  3 
.also  derives  fimdament.il  structural  [iropc'rties  of  C-Spacc  constraints,  in  particular, 
the  domains  ;md  domain  topology  of  C-functions  for  the  classical  Movers’  problem. 
We  call  these  domains  np])licability  constraints.  Chapter  •!  addresses  the  intersection 
problem  in  high-dimensional  C-Space:  how  to  construct  and  slide  along  intersection 
manifolds,  and  how  to  intersect  trajectories  with  C-surfac.es  and  a|)plic;ibility 
constraints.  We  demonstr;ite  the  form  of  the  intersection  manifolds  for  3?'*  X  SO{3) 
and  3?'^  X  -S'*.  Chapter  .5  discusses  algorithms  for  moving  through  rotation  space, 
and  for  decomposing  rotation  space  into  equivalence  classes  where  the  set  of 
.q)l)licablc  constraints  is  invariant.  In  cimpter  6,  we  extend  the  concept  of  the 
generalized  Voronoi  diagram  (which  Drysdale  (1983)  diTmed  for  the  plane)  to  the  six 


dimensional  C-Space  3?^  X  50(3),  to  provide  a  formal  and  constructive  definition 
of  the  C-Voronoi  Diagram,  or  CVD.  The  CVD  is  an  attractive  construction,  in  that 
it  contains  a  representative  component  for  each  “branch”  of  free  space.  Each  such 
component  is  a  submanifold  of  dimension  0  <  d  <  5,  called  a  Voronoi  manifold. 
We  will  derive  the  following  connection  between  intersection  manifolds  and  the 
CVD: 

Theorem:  ( The  Equivalence  Theorem  for  intersection  manifolds  and  the  CVD). 
Let  p  be  a  path  along  the  CVD.  p  lies  along  a  connected  chain  of  Voronoi 
manifolds,  Vi, .  . . ,  Vjt-  We  demonstrate  that  for  each  Voronoi  manifold  Vi,  there 
exists  an  equivalent  intersection  manifold  of  level  C-surfaces,  Furthermore, 
we  also  show  that  for  every  connected  chain  of  Voronoi  manifolds,  there  is  an 
equivalent  connected  chain  of  intersection  manifolds  (of  level  C-surfaces).  (The 
equivalence  we  demonstrate  is  actually  stronger  than  homotopic  equivalence, 
but  the  additional  details  are  too  complicated  for  this  chapter). 
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2 

A  Planning  System  for  the  Classical 
Movers’  Problem  with  Six  Degrees  of  Freedom 

In  this  chapter,  we  describe  the  design  and  implementation  of  a  planning 
system  for  the  classical  Movers’  pioblom  witli  six  degrees  of  freedom.  The  planning 
algorithm  required  the  solution  of  the  seven  “Representational  and  Algorithmic 
Questions”  listed  at  the  end  of  chapter  1.  The  solutions  to  these  problems  are 
presented  in  subsequent  chapters. 

In  this  chapter  we  will  simply  assume  that  these  problems  arc  solved,  and 
proceed  to  employ  the  solutions  in  constructing  a  planning  algorithm.  Of  particular 
importance  will  be  two  efl'cetive  procedures,  which  address  the  intersection  problem 
in  C-Space: 

(I)  Given  two  or  more  level  C-surfaces,  construct  their  intersection  manifold. 

(Chapters  3  and  4). 

(II)  Given  a  C-surfacc  and  a  trajec’ory,  Gnd  their  intersection.  Determine 

whether  the  intcrscctior\  lies  on  the  boundary  of  a  C-Space  obstacle.  (Chapters 

4  and  5). 

The  immediate  application  of  (I)  is  the  sliding  problem;  Ilow  to  slide  along 
one  level  C-surfacc,  and  how  to  slide  along  the  intersection  of  two  or  more  level 
C-surfaces. 


Using  the  point  navigation  operators  (chapter  1),  we  implenientcd  a  best-first 
search  algorithm  in  C-Space.  The  algorithm  has  nice  theoretical  properties  which 
include  completeness  (at  a  resolution).  This  chapter  describes  the  heuristic  search, 
with  particular  emphasis  on  the  heuristic  strategics  that  evaluate  local  geometric 
information,  and  on  the  interaction  of  these  strategies. 

2.1.  Definitions 


A  topological  space  M  is  called  an  n-dimensional  manifold  if  it  is  locally 
homeomorphic  to  3J".  A  chart  is  a  way  of  placing  a  coordinate  system  on  M:  if 
U  and  V  are  open  subsets  of  M,  two  homeomorphisms  f  :  U  —*■  f{U)  C  di"  and 
g  :  K  — >  (/(U)  C  3fJ"  have  C°°  overlap  if  the  maps 

fog-^:g{UC{V)-^f{Uf]V) 
gof-^  :f{UC\V)-^g{Uf]V) 

are  also  C*”  (that  is,  possessing  continuous  partial  derivatives  of  all  orders).  A 
family  of  pairwise  C°°-ovcrlapping  homeomorphisms  whose  domain  covers  M  is 
called  an  alias  for  M.  A  particular  member  (/,  U)  of  an  atlas  U  is  called  a  chart  (for 
the  atlas  U),  or  a  coordinate  system  for  U.  For  a  good  introduction  to  difTcrential 
geometry,  sec,  for  example,  (Spivak,  1979). 

In  this  thesis  we  usually  specify  charts  via  the  inverse  form  h  :  li  —r  M 
(where  ll  is  an  open  sub.set  of  3?")  with  the  understanding  that  it  is  the  inverse 
(or  set  of  local  inverses)  /i'  *  which  provides  the  family  of  charts  {(h“*,Vy,)},  for 
U,  Wi  =  h[ll).  As  an  example,  consider  the  map  h  that  specifics  a  chart  for  a  five 
dimensional  level  C-surface: 

h  :  SR'’  -»  SR'l  X  50(3) 

(  I  n  L\  /  ^S>!/  +  +  E/\  —  I  I  n  J.\ 

(y,z,i/>,  <?,</>)  ( - - - 

Here  the  E,  are  smooth,  real-valued  functions  on  50(,3),  that  is,  E,  :  {ijj,0,(l>)  — >  5R. 
The  inverse  map  h"  '  is  obvious,  and  provides  a  chart  for  the  five  dimensional 
subrnanilold  of  5R'*  X  50(.l).  In  subsequent  chapters  we  will  derive  such  charts,  in 
the  rorn\  of  h]  in  this  chapter,  we  will  take  them  for  granted. 


2.2.  Iiitrodiictiun 


\Vc  arc  now  ready  to  describe  a  planning  system  for  the  find-path  problem  in 
9?'*  X  iiO(d).  Tlie  algoritlim  lias  the  structure  of  .1  search  and  is  complete  (for  a 
given  resolution),  'fhe  basic  idea  is  as  follows:  we  arc  able  to  define  and  implement 
certain  local  operators.  When  applied  at  a  configuration  in  C-Spacc,  a  local  operator 
attempts  to  move  the  robot  in  a  specified  direction  until  cither  the  subgoal  or  an 
intervening  C-surface  is  reached.  The  local  operators  have  the  general  form 


M  ovc(X  :coTi  figuration,  indirection,  limiticon  figuration), 

and  arc  designed  to  return  X',  the  configuration  reached  in  direction  v,  and 
the  reason  for  stopping  (which  will  cither  be  “reached  subgoal”  or  the  name  of 
the  C-surface  which  halted  progress).  The  local  operator  assumes  that  X  is  in 
frec-spacc,  and  ensures  that  there  exists  a  collision-free  path  along  v  taking  the 
robot  from  configuration  X  to  X'.  Furthermore,  we  insist  that  limit  =  X  +  tv,  for 
some  positive  t.  In  general,  v  can  be  represented  as  a  tangent  vector  to  X  50(3); 
the  space  of  directions  is  clearly  locally  homcomorphic  to  SR**. 

Many  different  Move  operators  can  be  defined.  Let  X  =  (x,  ©).  We  will  restrict 
ii  to  be  cither  a  pure  translation 


t)  e  iR'*  X  { 0 } 

or  a  i)urc  rotation 

V  €  {  Ti/!’, -0,  f0, 

The  closure  of  these  operators  is  complete  for  the  space  of  configurations.  13y 
this  we  mean  that  in  the  absence  of  obstacles,  there  is  some  finite  secpience  of 
operators  which  carries  any  configuration  X  into  any  other  configuration  V.  It  is 
often  convi'nient  to  think  of  these  oper.ator.s  as  Tran.'ilule{.Y ,  ii,  x')  (where  u  £ 
and  x'  Is  a  goal  translation)  and  I{otnte[X ,ip,ip')  (where  p  is  an  angular  direction 


and  'p'  is  a  goal  angle).  The  theory  and  implementation  of  Translate  and  Rotate 
is  discussed  in  chapters  3,  4,  and  5. 

Given  the  local  operators,  we  can  dcGiie  more  sophisticated  local  strategies  for 
spatial  reasoning.  These  strategies  are  implemented  by  local  experts^  in  C-Space. 
hor  example,  one  local  expert  attempts  to  circumnavigate  C-Space  obstacles  by 
sliding  along  intersections  of  level  C-surfaccs.  Another,  “greedy”  expert  tries  to 
translate  or  rotate  straight  towards  the  goal.  A  local  expert  typically  examines  the 
local  geometric  environment  of  C-surfaces,  their  normals  and  intersections.  It  also 
takes  into  account  the  history  of  planning.  The  local  experts  can  be  thought  of  as 
issuing  “commands”  in  terms  of  the  local  operators.  Depending  on  the  results  of 
these  attempted  motions,  an  expert  may  issue  other  local  operator  commands,  and 
either  directly  invoke  or  leave  a  forwarding  message  for  another  local  expert. 

To  summarize:  a  local  operator  is  an  algorithm  for  moving  along  a  specific 
trajectory  until  a  constraint  is  encountered  (or  a  subgoal  is  reached)  .  A  local  expert 
is  a  strategy  for  choosing  the  trajectory  based  on  an  examination  of  the  history  of 
planning  and  the  local  geometry.  When  a  local  expert  chooses  a  trajectory,  it  calls 
on  some  sequence  of  local  operators  to  realize  it. 


'’rtic  Icrtn  lornl  i  Xpert  vv;us  broii'dil.  to  [oy  attention  in  disciisMions  witti  Van-I)iir  NKnyen  (NKnyeti 
Tonias  l.o/ano-l’erc'/,,  ami  Ibxlm-y  lirooks. 
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Figure  2.1.  Schctriiitic  illuatralion  of  the  “BunibJc"  strategy  (an  exhaustive  search).  A  fine 
six-dirtiensioiial  lattice  is  thrown  acros.s  C-Sjiacc.  By  exploring  from  one  configuration  to  its 
noighbor.H  in  the  lattice,  a  path  will  eventually  be  found,  if  one  exists  at  the  lattice  resolution. 
Fortunately,  it  is  also  possible  to  take  large  step.s  in  the  lattice,  and  simply  record  the  neighborhoods 
the  path  visits. 


2.2.1.  Planning  and  Search 


The  planning  algorithm  is  implemented  as  a  search  of  conGguration  space.  The 
search  constrvicts  a  graph  of  neighborhoods  which  have  been  explored.  (We  will  be 
more  precise  about  the  term  neighborhood  later).  Each  node  in  the  search  graph  is 
associated  with  a  configuration  and  contains  information  abou‘  the  local  geometry 
and  the  history  of  planning.  The  search  algorithm  chooses  a  node  for  exploration. 
Several  local  experts  arc  then  applied  at  that  node.  Each  expert  can  produce  a  new 
search  node.  All  of  these  arc  sons  of  the  explored  node,  and  arc  added  to  the  search 
cjueue.  The  new  sons  arc  connected  to  their  father  by  the  arcs  of  the  search  graph 
and  each  son  may  be  thought  of  as  an  exploration  from  the  father. 

If  at  any  point  in  the  search,  two  explorations  reach  the  same  neighborhood, 
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the  planner  attempts  to  merge  the  associated  nodes  into  one  node. 

The  search  algorithm  is  Best-First  (Nilsson  (1980))  with  the  metric  of  progre.ss 
established  as  distance  from  the  goal.  (This  requires  placing  a  metric  on  both 
translation  and  rotation  space).  Other  search  measures  (such  as  path  length,  or 
time)  would  also  be  possible,  and  an  A*  search  strategy  could  be  exploited  to  find 
optimal  paths.  In  practice  this  would  probably  require  adding  new  local  experts  in 
order  to  ensure  reasonable  performance. 

As  search  nodes  are  explored,  they  are  entered  in  a  priority  cpieiie,  called  the 
search  queue.  The  nodes  in  the  search  queue  are  ordered  by  the  search  metric. 
Some  search  strategies  we  discuss  require  two  search  queues:  when  the  primary 
queue  is  exhausted,  then  nodes  from  the  reserve  queue  are  explored. 

We  will  proceed  as  follows.  First,  using  the  local  operators  alone,  we  can  define 
a  complete  search  strategy  (at  a  given  resolution).  This  search  strategy  can  be 
considered  the  most  primitive  local  expert,  and  is  known  as  the  “Bumble  Strategy.” 
By  applying  the  Bumble  strategy  at  every  search  node,  we  are  guaranteed  to  find 
a  path  (at  a  given  resolution)  if  one  exists. 

Next,  we  will  define  more  complicated  local  experts  which  will  be  applied  to 
search  nodes  at  the  same  time  as  the  Bumble  expert.  These  experts  greatly  improve 
the  performance  of  the  planner. 


A  (Complete  Scarcli  Strategy 

A  search  node  is  associated  with  a  configuration.  ICvery  configuration  is  in  turti 
associated  with  a  neighhorhood  of  C-Spnee.  The  neigldjorhoods  form  a  partition 
(jf  ("  Space.  Since  many  conligurations  are  associated  with  one  neighborhood,  so 
several  search  nodes  may  have  configurations  lying  in  the  sattie  neigliborhood . 

Assume  tlie  neighborhoods  arc  “small.”  If  the  configurations  of  two  search 
nocles  are  in  the  same  neighborhood,  it  indicates  that  they  should,  if  possible,  be 
merged  into  one  node,  since  Uiey  are  close  together.  Hy  keeping,  trai  k  of  the  set  of 
explored  neighborhoods,  wc.  can  avoid  redundant  explorat  ions.  If  the  neig.hborhooils 
are  sufficiently  small,  then  tlie  search  will  be  complete  at  a  resolution  clo'-ely  rel.itiul 
to  the  neighborhood  si2e. 


(i)  A’,  llie  configuration  of  tlic  searcfi  node  is  mapped  to  L,  a  point  in  the  lattice. 

L  is  the  name  of  the  neighiiorhood  centered  on  L,  which  contains  X . 

(ii)  'I'he  unexplored  neighborhoods  adjacent  to  X{L)  are  found.  lOach  of  these 
neighborhoods  is  also  identified  by  a  central  lattice  point. 

(iii)  'I'lie  planner  attempts  to  move  to  each  of  the  unexplored,  arljaccnt  neighbor¬ 
hoods. 

(i)  has  the  effect  of  mapping  a  neighborhood  of  C-Space.  to  a  canonical  element 
(which  lies  on  the  lattice)  in  its  interior.  These  neighborhoods  decompose  3?'^  X  -S  0(3) 
into  equivalence  classes  with  the  same  canonical  clement.  When  a  neighborhood 
is  readied  for  the  first  time,  we  mark  its  lattice  point  as  explored.  The  search 
terminates  when  a  neighborhood  containing  the  goal  is  reached,  and  when  that 
exploration  can  be  connected  to  the  goal  configuration. 
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2.3.1.  Implementation  oT  Neighborhoods  and  Lattices 

In  principle,  it  is  possible  to  implement  the  lattice  as  a  six-dimensional  array 
(with  modular  indexing  for  the  rotational  dimensions).  In  practice,  for  any  fine 
resolution,  this  array  will  be  enormous,  and  very  sparse.  Although  an  adversary 
can  design  a  find-path  problem  for  whicli  our  planner  must  explore  the  entire 
lattice,  in  jiractice  this  does  not  occur.  However,  we  must  maintain  a  record  of  what 
neighborhoods  have  been  explored,  in  order  to  generate  the  unexplored  neighbors 
for  a  search  node.  Since  the  array  is  sparse,  we  will  employ  a  different  strategy. 

.\  partial  order  can  be  defined  on  lattice  points  by  considering  them  as 
six-diinei.sional  vectors.  This  order  has  no  particular  geometric  significance  for 
the  rotational  dimensions,  but  it  can  be  used  to  store  explored  lattice  points  in  a 
binary  tree.  Since  the  vast  majority  of  neighborhoods  are  never  explored,  the  tree 
it  typically  small,  even  for  fine  lattices.  To  mark  a  lattice  point  as  explored,  we 
insert  into  tli>‘  binary  tree,  'fo  find  wliether  a  lattice  point  has  been  ex{)lorcd,  we 
search  itie  tree. 

It  is  desirable  to  employ  a  fine  lattice  in  order  to  ensure  completeness  at  a  fine 
resolution,  'flic  use  of  a  binary  tree  to  record  explored  configurations  effectively 
removes  the  problem  of  lattice  size  for  storing  explored  configurations.  For  example, 
if  we  segment  C-Spacc  into  an  /V  X  Af  X  •••  X  A/  lattice,  then  an  array  would 
have  to  be  long.  Hut  the  binary  tree  need  store  only  the  explored  locations,  and 
(if  height-balanced)  can  access  any  leaf  in  0(log /V)  operations. 


If  tho  lattice?  resolution  is  fine,  then  the  planner  as  described  so  far  will  take 
very  small  steps  for  each  search  exploration,  d'his  has  been  remedied  as  follows: 
If  a  local  operator  is  invoked  to  find  whether  limit  may  be  attained  from  X  in 
direction  f,  it  must  effectivcily  intersect  a  path  in  direction  v  with  all  ('-surfaces.  It 
is  not  much  harder  to  firul  the  first  constraint  alonj',  the  path  p[t)  A  I  In  (even 
if  it  is  beyond  limit):  in  particular,  we  note  that  all  intersections  aloiiK  the  path  p 
may  be  sorted  on  distance  from  A".  'I'lic  complexity  of  litulinit  this  lirst  inti-rseel  ion 
along  p  is  independent  of  the  lattice  resolution  (since  the  intersection  algorillini  has 
nothing  do  do  with  the  lattice;  see  chapter  5).  VVe  can  “sainjjle”  the  portion  of  the 
path  which  Hits  in  free  space  at  the  lattice  resolution.  All  of  these  confi;',nrations 
are  then  marked  as  “explored”,  and  as  reachable  from  their  immediate  neighbors 
along  the  path.  Thus  they  form  a  connected  chain  in  the  lattice  along  the  path  p. 
While  all  these  configurations  are  iti  some  sense  sons  of  X ,  in  practice  we  will  select 
only  one  or  two  to  be  entered  in  the  primary  search  queue.  These  sons  might  be 
(1)  tho  son  which  is  closest  to  tlie  goal,  and  (2)  some  son  at  a  reasonably  large  step 
away  from  X.  This  step  size,  called  the  Bumble  resolution,  might  be  3  to  10  times 
the  lattice  resolution.  Tlie  other  sons  should  be  kept  on  a  reserve  queue,  which  can 
be  explored  when  the  primary  search  queue  is  depicted  or  exhausted. 

In  practice,  it  may  preferable  to  enter  ranges  in  the  exploration  tree,  for 
example,  to  record  that  all  lattice  points 

(i,  y,  z,  ip,  0,  (p)  <  L  <  (i  +  kdr,  y,  z,  ip,  0,  (p) 

(for  some  integer  k)  are  explored.  This  requires  keeping  an  exploration  tree  of  lines 
instead  of  configural  ions,  with  the  intent  of  minimizing  the  number  of  exploration 
tree  entries.  When  lines  arc  entered  into  the  tree,  they  may  be  merged  with  previous 
lines  to  form  connected  components  of  explored  regions,  'riiese  operations  are 
su[){)orted  by  hierarchical  subdivision  algorithms.  At  this  point  in  the  ex]n'rimcntal 
use  of  the  j)lanner,  it  is  still  too  early  to  tell  whether  this  optimization  is  necessary. 


Ill  practice  we  liave  liad  no  problem  In  selecting  a  very  fine  resolution  for 
the  lattice  (one  selects  a  line  lattice  resolution,  and  a  considerably  larger  Bumble 
resolution  or  step  size,  as  described  above).  This  lattice-based  strategy  is  not 
only  theoretically  complete  for  a  given  resolution,  but  has  also  been  used  to  find 
very  cornjilicated  paths  for  the  (>  degr(;e  of  freedom  classical  Mover’s  problem. 
However,  the  algorithm  has  an  “excessively  local”  flavor  -  it  is  clumsy  and  quite 
slow  when  employed  alone  (hence  the  strategy’s  name).  We  can  construct  much 
“smarter”  heuristic  experts  which  attempt  to  exploit  coherence  in  C-Space.  When 
these  experts  are  used  in  conjunction  with  the  Bumble  strategy,  we  obtain  a 
planner  which  is  not  only  complete,  but  which  can  solve  complicated  problems  in 
a  reasonable  amount  of  time.  We  continue  to  find  the  lattice  useful  for  recording 
the  planner’s  explorations  by  the  local  experts. 

2.3.2.  Keeping  Track  of  Connectivity 

Suppose  a  subsequent  exploration  reaches  the  same  neighborhood.  There  are 
two  choices,  which  we  call  the  mark  algorithm  and  the  connect  algorithm: 

The  Mark  Algorithm.  Discard  the  exploration,  since  the  neighborhood  is  already 
explored.  In  practice,  the  mark  algorithm  often  suffices  for  path-finding.  The  mark 
algorithm  computes  a  directed,  spanning  tree  T  of  explored  neighborhoods,  which 
is  rooted  at  the  start  configuration. 

The  Connect  Algorithm.  Connect  together  the  search  nodes  for  all  explorations  to 
that  neighborhood.  The  connect  algorithm  is  more  complicated,  and  reejuires  the 
following  bookkeeping  (see  figure  2.3).  Let  .V  be  a  neighborhood  of  3?'^  X  50(3), 
and  L  ^{hl)  be  a  lattice  point  which  is  the  c.anonical  element  for  M .  Suppose  X  is 
an  exploration  of  M ,  i.c.,  X  C  M  is  the  final  configuration  in  some  motion  reaching 
M ■  Let  •s(X)  denote  the  search  node  for  X.  (If  X  is  the  first  exploration  of  .V,  then 
create  a  search  node  s(/y)  for  L).  Determine  whether  there  exists  a  path  from  X  to 
L  (using  the  local  operators).  If  so,  connect  s(X)  and  s(L)  together. 

The  connect  algorithm  computes  a  more  complete  connectivity  graph  for 
the  neighborhoods  of  C-Spacc..  ft  computes  .an  undirected  graph  II  of  explored 
neighborhoods,  which  may  contain  cycles.  As  long  as  //  is  connected,  then  T 


79 


Figure  2. 3.  'I'lic  lattice  point  L  is  at  the  center  of  a  neighborhood  >/  of  C-Space.  Search 
ex()lor:itions  arrive  at  configurations  X  and  Y  in  X.  The  planner  attempts  to  find  a  path 
roiinectiiig  A'  and  Y,  by  tryiiig  to  connect  both  configurations  to  L. 

is  a  spanning  tree  for  II,  and  the  mark  algorithm  is  complete  for  planning  a 
connected  path  along  II.  However,  not  all  planning  strategies  admit  this  kind  of 
“connected  planning.”  In  particular,  when  we  consider  strategies  which  construct 
partial  paths  and  pla  ining  islands  (which  may  later  connect  up),  the  connect 
algorithm  is  necessary.  (See  the  Suggestor  strategy,  below,  for  an  example). 


2.3.3.  Discussion  of  the  nurnble  Strategy 

Suppose  the  lattice  spacing  is  dy  and  du  in  the  translational  and  rotational 
dimensions.  Then  the  adjacent  lattice  points  to  L—  [x,y,  z,rp,0,  (f>)  will  be: 
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(i  ±  dr,y,z,tl},0,(f>) 

[x,y  ±  dr,z,rl),0,4>) 

{x,y,z  ±  dr,tl},0,<l>) 

(x,  y,  z,rp  ±  dit  (mod  27r),  0,  (f)) 

[x,  y,  z,iIj,0  ±  dn  (mod  27r),  <^) 

(x,  y,  z,  ■>f),0,4>±  dn  (mod  2tx)) 

Each  adjaccnl  lattice  point  is  tlie  center  of  a  neighborhood  of  configurations  which 
is  contiguous  to  tlie  neighborhood  of  [j.  Each  such  neigliborhood  can  be  reached 
(if  it  is  in  free  space  and  there  is  no  intervening  C-surface)  by  the  local  operators 
Translate  and  Rotate.  Since  there  are  12  neighbors  for  each  lattice  point,  vve  have 
found  it  inadvisable  to  explore  them  all  for  each  search  no<le  expansion.  Instead, 
the  set  of  unexplored  adjacent  neighborhoods  is  ranked  (in  terms  of  proximity 
to  the  goal),  and  motions  towards  the  top  translational  and  rotational 
neighbors  arc  attempted.  (Typically,  kx  ^  3  and  k^  2).  If  the  node  is  reexplored 
later,  motions  toward  kx  +  k^  more  of  the  unexplored  neighbors  will  be  attempted 
(if  there  are  that  many  left).  When  using  the  mark  algorithm  (above),  we  say 
an  ex()loration  is  successful  if  it  reaches  a  new  (unexplored)  neighborhood.  If  an 
exploration  is  successful,  then  a  new  search  node  is  created  and  the  neighborhood 
is  marked  as  explored.  Since  the  neighborhood’s  “name”  is  its  lattice  point,  this 
simply  corresponds  to  marking  the  lattice  point.  Whether  successful  or  not,  all 
explorations  arc  recorded  at  the  parent  search  node  so  that  they  will  not  be  tried 
again. 

Suppose  X  is  a  configuration  in  neighborhood  M{h),  with  associated  lattice 
point  L.  d'he  unexplored  adjacent  lattice  points  to  L  indicate  a  set  of  subgoals  to 
be  attained  from  X .  'Pin;  numble  strategy  ranks  these  snbgoals,  choost?s  some  of 
them,  and  .selects  t.raji'ct ories  which  may  attain  them.  The  local  operators  are  then 
employc'd  to  (try  to)  realize  the  selected  traject.orics.  These  explorations  arc  then 
recorded  so  that  only  new  exjjlor.it ions  will  be  pursued  in  the  future.  Note  that 
the  planiHjr  is  not  consiraincd  to  move  along  the  lattice,  arul  that  although  the 
subgoals  lie  on  the  lattice,  the  motion  from  X  to  any  subgoal  does  not,  unless 
X  ^  L. 

The  local  exfjorts  are  consich'rably  more  sophisticated  t  han  I  he  Humble  strategy. 
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'riu'ir  subgoals  need  not  lie  on  the  lattice,  and  the  motions  specified  to  the  local 
ojx'rators  need  not  lie  along  tfie  lattice.  'I'he  lattice  is  still  employed  to  keep  track 
of  the  planning  history  and  the  connectivity  of  explored  neighborhoods. 

Clearly,  the  arcwisc-connectcd  sets  of  lattice  points  are  closed  under  the 
opiirators  Translate  and  Rotate.  If  a  path  exists  at  the  lattice  resolution,  then 
the  search  is  guaranteed  to  find  it.  We  see  now  exactly  what  the  resolution  for 
this  find-path  algorithm  is:  by  choosing  a  sufliciently  fine  lattice,  the  algorithm  is 
(trivially)  complete  at  the  lattice  resolution.  As  we  saw  above,  we  can  choose  a  very 
fine  lattice  with  little  computational  overhead.  One  final  point;  the  start  and  goal 
configurations  may  not  lie  directly  on  the  lattice.  This  is  not  a  problem,  however, 
since  the  local  operators  can  ensure  that  there  exists  a  path  from  the  start  and  goal 
to  the  nearest  lattice  point. 

2,4.  Local  Experts  for  the  Find-Path  Problem 

2.1.1.  Path  IManning  versus  Continuous  Intersection  Detection:  Why  We 
Need  Local  ICxperts 

The  Translate  and  Rotate  oper.ators  detect  collisions  along  continuous 
trajectories.'^  Civen  these  operators,  it  is  possible  to  devise  a  complete  path-planning 
algorithm  based  on  something  like  the  Bumble  strategy,  above.  However,  while 
com[)lete,  this  is  not  a  particularly  good  algorithm,  in  that  it  says  nothing  about 
how  or  when  the  operators  should  be  .applied.  The  domain  of  the  operators  is 
large  and  foi  realistic  path  planning,  it  is  necessary  to  know  where,  and  in  what 
directions  to  apply  them. 

Algorithms  which  can  detect  intersections  with  obstacles  for  a  robot  following 

a  continuous  trajectory  say  nothing  about  how  to  plan  these  trajectories. 

However,  they  can  be  used  to  find  a  path  by  exhaustive  search. 

'fhe  Translate  and  Rotate  o[)erators  use  the  constraints  in  C-Space  to  detect 
collisions.  However,  these  constraints  can  also  be  employed  to  plan  paths.  In 
chai)ter  1,  we  proposed  an  idealized  j)lanner  which  constructed  the  intersection 
■^'I'lpis  (liMCiissioii  ;itsi>  IroMs  for  I  lie  j'eiieral  Move,  operator. 


manifolds  of  level  C-surfaccs,  and  slid  along  these  manifolds  to  navigate  around 
C-Space  obstacles.  Such  a  planner  could  exploit  coherence  in  configuration  space: 
by  examining  C-Space  constraints  an  algorithm  can  be  devised  for  intersecting 
and  sliding  on  C-surfaces  to  circumnavigate  C-Space  obstacles.  In  the  following 
sections,  we  describe  a  planner  which  approaches  the  idealized  planning  algorithm 
of  chapter  1.  The  local  experts  are  strategies  for  reasoning  about  the  local  geometry 
of  configuration  space,  and  for  exploiting  geometric  constraints  to  plan  collision-free 
paths.  When  applied  to  a  search  node,  each  local  expert  examines  the  local  geometry 
and  history  of  planning  to  propose  one  or  more  path  segments.  Each  path  segment 
is  realized  by  means  of  the  local  operators,  which  ensure  that  a  collision  free  path 
exists. 

2.4.2.  Designing  Local  Experts 

In  the  exploration  tree  of  C-Space  neighborhoods,  we  have  seen  one  type  of 
information  that  must  be  maintained  for  planning.  In  designing  local  experts,  we 
must  address  the  following  questions: 

(i)  What  constitutes  a  local  description  of  a  (level)  C-surface? 

(ii)  What  information  should  be  stored  at  a  search  node? 

(i)  can  be  stated,  “What  constitutes  a  sufficiently  rich  description  of  the 
local  geometry  in  C-Space  to  allow  robust  local  experts?”  (ii)  relates  more  to  the 
history  of  planning,  and  the  connectivity  of  the  explored  search  neighborhoods. 
For  example,  we  want  to  record  the  results  of  previous  applications  of  experts  at  a 
.search  node,  and  the  adjacent  nodes  in  the  search  graph. 

I'lic  Local  Description  of  a  C-surfacc 

A  C-surfacc  has  a  normal  at  point  A"".  Motions  tangent  to  the  C-surface  at  X 
will  have  instantaneous  velocities  orthogonal  to  the  normal.  We  must  characterize 
the  normal  and  tangents  to  a  C-surf.acc  in  order  to  plan  trajectories  which  slide 
along  it. 

Let  /  be  an  appl’cable,  positive- valued  C-function  at  X .  We  can  check  that  /  is 
non-redundant  at  X  (sec  chapter  6);  altern.atively,  wc  may  heuristically  assume  /  is 
non-redu:ulant  if  its  value  at  X  is  small.  We  wish  to  develop  a  local  characterization 


of  /  at  X,  that  is,  of  the  level  C-surface  5  =  {  K  |  f{Y)  —  f{X)}  about  X.  We 
should  think  of  S  as  the  kernel  of  the  auxiliary  function 


fx  :  !»'*  X  SO(3)  ^  3? 

Y  K.  f{Y)  -  f{X). 

The  local  characterization  will  have  two  parts,  one  of  which  is  invariant,  and  one 
of  which  will  change  for  different  subgoals.  The  invariant  part  of  the  description  is 
a  pair, 


{/m,  V/) 


consisting  of  the  value  of  /  at  X  and  the  normal  to  S  at  X .  Now,  since  9?^  X  SO(3) 
is  not  a  vector  space,  the  normal  V  f{X)  to  5  at  X  will  de[)cnd  on  the  Riemannian 
metric  defined  on  the  tangent  space  at  X".  We  will  employ  a  metric  which  admits 
construction  of  V/(X)  using  the  partial  derivatives  of  /  at  X,  with  respect  to  the 
parameterization  of  C-Space.  Hence  if  rotations  are  parameterized  by  Kuler  angles, 
then  = 


Assume  that  V /  is  normalized  to  be  a  unit  vector.  We  now  wish  to  characterize 
the  relationship  of  the  C-surface  to  some  subgoal,  G:  this  rcc^uircs  some  way  of 
talking  about  directions  in  9?’  X  »90(3).  Specifically,  we  wish  define  a  “vector” 
algebra  on  configurations,  such  that 


and 


lim 
a  -X 


|[C-X||  =  0 


lim  (G-X).(G'-X)  =  1. 

it  ~*Lt 

These  equations  express  the  vector  space  characteristics  which  are  required  for 
our  computations  on  tangent  vectors.  To  construct  this  algebra,  it  is  possible  to 
define  a  field  of  inner  products  over  9?'*  X  50(3),  i.e.,  to  define  an  inner  product  on 
the  tangent  space  to  each  point.  'I'lius  9?'*  X  50(3)  is  a  Riemannian  mani.f’old  (see 
Erdmann  (1984)).  If  two  tangent  vectors -i.e.,  directions -are  applied  to  the  same 
point,  this  inner  product  allows  us  to  talk  about  the  angle  between  two  such  tangent 
vectors,  or  of  the  angle  between  an  arbitrary  tangent  vector  to  9f  ’  X  50(3)  and  the 


normal  to  a  C-surfacc.  However,  the  inner  product  is  somewhat  arbitrary  for  our 
application.  Alternatively,  we  could  also  construct  geodesics  on  the  3-sphere 
with  antipodal  points  identiOed.  These  approaches  arc  probably  too  elaborate  for 
a  heuristic  strategy. 

Heuristics  for  Evaluating  Directions  in  the  Tangent  Space 

A  basic  issue  is  that  placing  a  metric  on  a  non-abelian  group,  such  as  50(3), 
is  a  difficult  problem.  We  will  demonstrate  the  metric  that  our  planner  employs, 
and  then  show  that  it  is  adequate  for  this  application.  In  particular,  the  metric 
is  adequate  when  applied  to  three  one-dimensional  slices  of  50(3).  (These  are  the 
slices  considered  by  the  Rotate  operator).  Note,  however,  that  a  metric  may  also 
be  derived  by  representing  rotations  as  unit  quaternions.  In  this  case,  the  metric  is 
obtained  by  considering  rotations  as  points  on  embedded  in  SfJ'’  (IVou  1983). 

Suppose  we  employ  rotation  matrices  to  represent  rotations.  (The  implemented 
planner  uses  Euler  angles).  If  we  are  willing  to  tolerate  singularities  in  the 
representation,  it  is  oltcn  convenient  to  identify  a  rotation  matri-x  in  50(3)  with 
the  vector  of  three  angles,  (^,0,^)  which  determine  it.  The  angles  {rp,0,<i>)  form  a 
three  dimensional  angle  apace,  Q^.  The  rotation  matrix  corresponding  to  [ipiOfCj))  is 
of  course  (The  singularities  induce  an  equivalence  relation  on  where 

two  points  in  angle  space  are  cqu.al  when  the  rotation  matrices  they  determine 
arc  equal).  Most  of  the  time,  the  identification  of  50(3)  with  docs  not  lead 
to  problems.  However,  when  wo  wi;:h  to  compute  directions,  and  differences  of 
configurations,  it  is  necessary  to  di.stinguish  between  50(3)  and  Q^. 

We  can  state  this  more  concisely  as  follow.s:  50(3)  is  a  tliree  dimensional 
manifold.  The  mapping  R.  from  f'iuler  .ingles  to  rotation  matrices  is  a  chart  for 
50(3): 


R  :  Q  '  -  >  50(3). 

VVe  typically  describe  a  rotation  .?((-))  t  50(3)  by  its  chart  coordinates  [t{),0,(j>)  = 
0  G  Q*.  'I'his  makes  it  convenient  to  identify  0  with  -?(0),  so  that  in  general, 
instead  of  de.iling  witli  the  manifold  directly,  we  will  work  with  a  chart  for  the 


8.') 


inaniFold.  In  this  section  alone,  however,  we  must  distinguish  between  the  domain 
and  image  of  Z. 

We  can  compute  a  direction  in  3?'*  X  SO[?>)  by  simply  subtracting  two 
configurations  (of  course  the  angles  must  be  subtracted  (mod  2n))  to  yield  a 
six-dimensional  direction  vector.  Using  (his  arithmetic,  the  goal  direxlion  is  denoted 
G  —  X .  We  will  use  the  convention  that  the  first  three  coordinates  of  G  —  X  arise 
from  3?'^,  and  the  second  three  coordinates  arise  from  Q^. 

Let  C  —  {GxtGi))  and  X  —  (Xi,X0).  Since  G  -  A'  is  clearly  well  defined 
when  G  and  X  differ  only  by  a  translation,  assume  that  G  and  X  dilfer  only  by  a 
rotation.  Assume  further  that  rotations  are  represented  by  Euler  angles.  Note  that, 
in  general  G  —  X  is  not  a  rotation  which  carries  the  moving  object  at  orientation 
G  into  the  moving  object  at  orientation  X.  However,  G  —  X  does  represent  the 
difference  in  orientation,  i.c.,  it  specifies  a  displacement  in  angle  space  which  will 
carry  G  into  X.  For  example,  if  Ge  —  (45°,  50",  90°)  and  Xe  =  (45°,  45°,  45°)  then 
there  arc  rotation  matrices  Z{G(j)  and  .C(Xo)  corresponding  to  each  of  G9  and 
X0.  (We  use  degrees,  not  radians  in  this  example,  since  the  sy.ubol  tt  will  soon  be 
used  for  a  projection  map).  Note  that 


je(45°,50°,90°)  7^  ;2(45°,45°,45°);2(0°,5°,45°), 
where  ZZ'  indicates  composition  of  rotations.  However,  the  path  in  angle  space 

P(0  =  +  ^(^0  ~  -^9) 

=  (45°,  45°,  45°)  +  «(0°,  5°,  45°) 

(for  t  G  [0,  1])  will  work,  since  it  corresponds  to  the  rotational  path 

Z{p{t))  ^  Z{Xe  +  ((Ge-Xo)) 

=  ^((45°, 45°, 45°)  4-  <(0°, 5°, 45°)). 


Consirlering  configuration  space  as  the  product  space  of  the  translation  space 
and  the  angle  space,  we  sec  that  G  —  X  is  well  ddined.  Ge  —  Xe  specifies  a 


direction  and  a  distance  to  be  traveled  in  angle  space  in  order  to  carry  into  (j0. 
Furthermore,  along  the  path  from  to  Cy,  the  corresponding  rotations  specified 
by  the  angle  space  trajectory  p  are  well  defined.  For  all  6'  6  X  50(3),  we  will 
treat  the  space  of  directions  C  —  X  as  the  tangent  space  Tx  to  X  SO{'i)  at  X . 
Properly,  T\  is  the  product  space  of  the  tangent  space  to  9?'*  at  Xj:,  and  the  three 
dimensional  angle  space  Q^. 

We  now  define  a  map  from  Ty  X  Tx  to  the  plane,  which  will  function  in  place 
of  an  inner  product.  First,  define  the  natural  projection  maps  from  7\-  onto  its 
factor  spaces: 


Tx  -*■ 

(G-X)^  {G^  -  X^) 

^0  :  Tx 

{G-X)^{Gq-Xq). 

Let  u  ■  V  denote  the  standard  inner  product  on  9?’,  for  vectors  u  and  v.  If  u 
and  V  are  p>  -jeetions  (under  7rjj,i)  of  direction  vectors  in  Tx ,  we  say  that  u  and  v 
are  translationally  orthogonal  if  u  •  u  =  0.  Let  6  Q^.  Assume 

the  each  pair  of  angles  q,-  and  Wi  (for  i  =  1,2,3)  is  normalized  so  that 

l^,-  —  ty,!  <  180°. 

(Note  that  this  normalization  is  critical).  Now,  define 

=qiwx  +  q2W2  +  q^w^. 

nq  will  function  in  place  of  an  inner  product  on  Q'\  We  say  that  two  rotational 
directions  q  and  w  are  rotationally  orthogonal  if  n(^{q,  w)  =  0. 

We  may  now  define  <Fx,  which  will  function  in  place  of  an  inner  product  on 
7'x .  First,  let 

D  =  G-X 
D'  =  G'  -X. 


l'..  ^  . 


Assume  that  Dx,  Z)',  Df^,  and  Dy  are  all  normalized  to  be  length  1  (where  the 
length  of  Dij  is  deGned  as  D^))i).  Finally, 

:  Tx  X  Tx  - 

(^D,  D']  H-.  nQ(ne{D),  ttbID')) j. 

So  4>_v  yields  a  pair  consisting  of  the  dot  product  of  the  translational  components 
of  the  direction  vectors,  and  the  tiq  product  of  the  rotational  direction  vectors.  If 
<Px{D,  D')  =  (0,0),  we  say  that  D  and  D'  are  orthogonal  directions  in  the  tangent 
space  Tx-  Note  that  two  directions  arc  orthogonal  if,  and  only  if,  their  translational 
components  are  orthogonal  and  their  rotational  components  are  orthogonal. 

This  discussion  extends  naturally  to  other  representations  for  rotations.  For 
example,  if  spherical  angles  (Kane  and  Levinson  (1978))  are  used,  then  the  difference 
in  orientation  is  the  rotation  carrying  G  into  X,  that  is.  Go  —  is  a  rotation 
carrying  the  moving  object  at  orientation  Xq  into  the  moving  object  at  orientation 
G'0.  Wc  should  stress  that  the  natural  Riemannian  inner  product  (Frdmann  (1984)) 
could  be  used  instead  of  <I>x-  This  would  cotnplicate  the  representations  employed 
in  subsequent  chapters,  ^x  Sfid  ’’^-Q  heuristic  measures  on  directions  in  Tx-  We 
will  later  di.scuss  why,  for  our  purposes,  they  are  good  heuristic  measures. 

Evaluating  Normahs  and  Gradients  to  C-Surfaces 

The  local  description  of  a  C-surface  relative  to  some  subgoal  is  designed  to 
address  the  following  qualitative  questions: 

(i)  Is  the  C-surfacc  locally  tangent  or  locally  orthogonal  to  the  goal  direction? 

(ii)  Is  the  C-surface  locally  orthogonal  to  any  rotational  motion? 

Recall  that  a  level  C-surface  kcr  /  is  described  by  a  real-valued  C-function  /. 
Assume  that  normals  and  tangent  vectors  arc  appropriately  normalized.  Question 
(i)  may  be  resolved  by  examining 

<I>x((G-n  V/(X)).  (2.1) 

When  (2.1)  approaches  (0, 0),  we  say  that  kcr  /  is  locally  tangent  to  the  goal  direction. 
Note  that  (2.1)  makes  sense:  /  maps  parameters  of  the  form  [x,y,  z,il>,0,(f))  to  real 


numbers,  and  hence  the  gradient  of  /, 

V9x’  dxj’  dz’  dtp’  dO’  d<p) 

is  clearly  a  direction  in  Ty  ■ 

We  will  also  employ 


7r,,e.(G-X).7r3j:.(V/(X)), 


(2.2a) 


When  (2.2a)  approaches  0,  we  say  that  ker  f  is  (locally)  translationally  tangent 
to  the  goal  direction.  Symmetrically,  when  (2.1)  (resp.  (2.2a))  approaches  (l,J) 
(resp.  l),  we  say  that  ker  /  is  locally  orthogonal  (resp.  translationally  orthogonal) 
to  G  —  X.  A  similar  calculation  yields  the  rotationally  tangent  and  orthogonal 
C-surfaces  to  the  goal  direction: 


nQ{7re{C  -  X),  7ro(V/(X))). 


(2.26) 


Why  <I>,Y  Good  Heuristic  Measures 

Suppose  that  the  rotational  direction  is  along  one  of  the  axes.  (Let  us  say  the 
direction  is  p).  To  tell  whether  a  C-surface  is  rotationally  orthogonal  (or  tangent) 
to  the  p  direction,  we  simply  examine  the  magnitude  of  which  can  be  obtained 
directly  from  Vf{X).  I'his  is  because 

nqiP,  /r(.)(V(/(X))))  =  nQ((0,0,  1),  —))  . 

In  other  words,  the  map  uq  need  not  be  employed.  Since  the  implemented  Rotate 
operator  moves  along  the  rotational  axes  in  directions 


u  €  {  +V',  -'0,  -+0,  -0,  l4,  -0  }, 

this  is  the  most  common  but  not  the  only --  test  for  rotationally  ortlu  gonal  (or 
tangent)  C-surfaccs.  'I’his  information  is  used  by  the  rotation  experts  to  choose 
rotatif)nal  subgoals  that  move  away  from  G-surfaccs. 


Description  of  a  Search  Node 


The  following  inforniation  is  stored  at  each  search  node.  Lazy  evaluation  is 
implemented  so  that  some  of  these  objects  (for  example,  the  set  of  all  applicable 
C-surfaces)  may  not  be  c(mi[)utcd  until  they  are  required. 

(i)  The  configuration  X  of  the  search  node. 

(ii)  I'lie  lattice  point  for  X ,  which  is  the  unique  identifier  for  the  neighborhood 
about  X . 

(iii)  The  applicability  set  at  X . 

(iv)  A,  the  set  of  non-redundant  constraints  at  X ,  sorted  on  increasing  value.  The 
non- redundant  constraints  may  be  approximated  by  the  applicable  constraints 
having  small  positive  (or  zero)  values  at  X . 

(v)  The  parent  node. 

(vi)  The  From- Direction  (The  direction  traversed  from  the  parent  node  to  this 
node). 

(vii)  The  sons  of  this  node.  These  include  “unsuccessful”  exidorations  which  did 
not  reach  a  sub-goal,  or  which  reached  a  previously  exi)lore(l  ruiighborhood. 

(viii)  The  C-surfaces  on  which  X  lies  which  also  bound  C -Space  obstacles,  that  is, 
all  f  E.  A  such  that  f{X)  =-  0  and  ker(/)  bounds  a  C-Space  obstacle  at  X. 

(ix)  An  fCxplanalion  of  how  this  node  was  reached.  An  explanat  ion  typically  includes 
the  name  of  the  local  expert  that  planned  the  move,  and  enough  information 
to  reconstruct  the  move.  For  example,  the  experts  which  slide  along  level 
C-surfaces  leave  an  explanation  containing  the  names  of  the  constraints,  their 
levels  at  the  parent  node,  and  the  parameterization  chosen  for  the  intersection 
manifold. 

Much  of  the  information  stored  at  a  search  node  is  used  to  record  the  history 
of  the  planning.  An  expert  which  planned  the  move  to  a  search  node  s  will  not  be 
applied  again  with  the  same  parameters.  As  an  example,  consider  the  Intersection 
expert,  which  attempts  to  slide  along  intersection  manifolds,  and  the  Greedy  expert, 
which  attempts  to  move  straight  towards  the  goal.  We  discuss  these  experts  in 
more  detail  iti  the  next  section.  If  applied  to  s,  the  C-surface  intersection  cx[)ert 
will  not  attempt  to  construct  and  slide  along  the  same  intersection  manifold 
which  led  to  .s,  unless  it.  can  slide  in  a  different  direction  along  the  intersection 
manifold.  Hy  recording  l.h<'  I'rom- Direction  for  a  node,  the  [)lanner  caii  avoid 
repeating  unfruitful  explorations.  In  particular,  different,  experts  can  advise  motion 
in  the  sarin;  direction;  thus  a  particular  intersection  manifold  may  point  in  the 
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same  direction  which  was  previously  (or  simultaneously)  attempted  by  the  (Jrcedy 
expert.  Wliether  successful  or  not,  reexploration  in  this  direction  may  be  avoided  by 
examining  the  From-directions  of  (he  sons  of  s.  An  additional  constraint  is  provided 
by  the  From-Dircction  of  s  itself:  there  is  typically  no  point  in  exploring  back  in 
the  direction  we  came  from.  The  process  of  leaving  information  for  some  expert 
which  may  be  applied  in  the  future  is  known  as  “forwarding.”  As  we  sliall  see,  the 
performance  of  one  expert  can  provide  strong  hints  as  to  what  expert  should  be 
applied  next. 

The  planner  computes  local  descriptions  for  the  C-surfaces  in  yi.  Naturally,  parts 
ol  these  descriptions  will  change  for  diflcrent  subgoals.  Ttie  local  characterizations  of 
C-surfaces  allow  the  planner  to  find  the  set  of  C-surfaces  to  which  the  goal-direction 
is  tangent  (or  orthogonal)  as  described  above.  When  a  planning  direction  is  chosen, 
these  C-surfaces  clearly  provide  strong  constraints. 

We  are  now  ready  to  discuss  the  experts  themselves.  The  Bumble  strategy  is 
also  applied  at  each  node,  since  it  is  a  guarantee  of  completeness.  In  light  of  the 
previous  discussion,  sve  will  omit  any  discussion  of  the  detection  and  pruning  out 
of  explorations  in  unfruitful  directions  (as  determined  by  the  planning  history). 
Wc  will  consider  the  application  of  particular  experts  to  a  search  node  s  (at 
configuration  X)  which  has  parent  .sq. 


2A.‘A.  Tho  Croody  ICxport 


The  greedy  expert  attempts  to  translate  or  rotate  directly  towards  the  goal, 
d'he  expert  is  necc'ssary  as  an  “end-game”  strategy,  in  order  to  close  in  on  a 
jrarticular  suhgoal  without  worrying  about  finding  the  apiirojiriate  intersection 
manifold.  'I'he  Greedy  expi'rt  illustrates  two  imjiortant  heuristics:  for  mar  ding  and 
backing  off.  Suppose  tlie  greedy  expert  translates  from  a  parent  node  so  to  a 
son  s.  An  a[)propriate  explan.ation  for  the  move  will  he  left  at  s.  If  the  same 
suhgoal  is  intact  when  the  planner  explores  .s,  the  greedy  expert  will  not  atternjrt 
translation  again.  Instead,  the  rotation  expert  (see  below)  might  he  invoked.  The 
effect  is  one  of  translating  until  an  obstacle  is  hit,  and  then  rotating  to  get  around 
it.  .Alternatively,  the  sliding  expert  (which  slides  along  levid  C-surfaces)  might 
be  invoked.  I'his  coupling  of  experts  is  termed  the  “liit  and  slide”  strategy  (see 
figure  2.-1).  However,  the  planner  does  not  directly  recurse  by  calling  the  sliding 
expi'rt  immediately  after  the  greedy  expert.  Instead,  a  suggestion  is  left  by  way  of 
explanation  at  s,  and  when  .s  is  explored  in  the  search,  the  appropriate  follow-up 
expert  is  invoked.  The  exact  choice  for  which  expert  is  invoked  will  depend  on 
the  history  of  planning  (tyjjically,  what  neighborhoods  and  directions  have  been 
explored  from  .s'o  and  ,5),  and  on  the  local  geometry  of  C-surfaces  about  s. 

Suppose  that  all  exports  moved  the  robot  as  far  as  they  could,  that  is,  moved  until 
a  constraint  was  hit  and  left  the  robot  touching  the  constraint.  This  could  result  in 
jiimming  the  robot  up  against  many  C-surfaces  at  once.  It  can  (rrove  V('ry  diflicult 
to  extricate  the  robot  froiti  this  logjam  situation.  In  fact,  it  is  usually  not  preferable 
to  move  all  the  way  up  to  an  obstacle.  Instead,  we  wish  to  detect  this  intersection 
with  a  iilanned  traj('ctory  p,  ;ind  then  back  off  from  the  obstacle  boundary  (along 
}>].  d’hus  if  ])[{])  -  ,V  aiul  p(l)  --  Y  is  the  first  intersection  of  p  with  C-Spacc. 
obst.-iel'-  boundary,  then  it  makes  good  sense  to  move  to  /)((). S),  'I'his  has  the  effect 
of  le.iving  the  rfjbot  in  the  channel  liet.ween  obstacles  in.'.le:id  of  jamming,  it  up  in 
corners.  Of  course,  if  it  is  m-cessary  to  move  to  p(.!)r))  then  the  greedy  and  Humble 
strate;’ii's  will  idtimately  converge. 
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2.4.4.  The  Intcrscclion  Expert 

The  mathematics  of  intersection  manifolds  in  5?®  X  50(3)  is  presented  in 
chapters  3  and  4.  The  intersection  expert  attempts  to  find  two  C-surfaces  in  A 
whose  intersection  manifold  contains  a  path  which  makes  progress  towards  a  subgoal. 
The  path  may  be  a  pure  translation  or  a  pure  rotation.  We  will  begin  by  describing 
the  process  of  finding  a  translational  path  which  slides  along  an  intersection 
manifold.  First,  all  C-surfaces  in  A  which  are  nearly  translationally  tangent  to  the 
goal-direction  are  selected.  We  select  the  first  few  of  these  which  have  the  smallest 
value  at  X.  Ideally,  these  are  the  closest  non-redundant  constraints  at  X.  Call 
this  set  A' .  The  explanations  for  the  moves  from  sq  ^  ^  and  from  s  to  any  sons 
of  5  will  yield  a  set  of  previously  explored  intersection  manifolds.  (An  intersection 
manifold  may  be  identified  by  the  name  of  the  intersected  C-surfaces,  their  levels, 
and  the  chosen  parameterization).  The  C-surfaces  in  A'  are  pairwise  intersected 


(see  chapter  4),  after  appropriate  pruning  as  indicated  by  previously  explored 
intersection  inanirolds.  ICach  intersection  manifold  (kcr  /)  n(kcr  g)  is  constructed.  A 
translation  or  rotation  vector  v/^g  is  chosen  such  that  the  path  p/,g{t)  =  X  +  t^f,g 
slides  along  the  intersection  manifold  of  the  two  level  C-surfaccs  kcr  /  and  kerg 
at  X.  The  intersection  expert  then  selects  the  direction  Vf  g  which  is  closest  to 
the  goal  direction  (and  which  is  not  pruned  out  by  consideration  of  the  planning 
history).  Suppose  Vfg  is  a  pure  translation.  The  local  operator  Translate  is  called 
to  move  from  X  in  direction  vj^g  until  a  C-surface  is  struck'*  or  the  point  on  the 
trajectory  pj^g  which  maximizes  proximity  to  the  goal  is  reached. 

Now,  suppose  vj^g  is  a  pure  rotation.  Our  experimental  implementations  have 
intersected  two  C-surfaces  ker  /  and  ker  g  to  yield  pure  rotational  paths  sliding 
along  the  intersection  manifold  of  ker  /Hkerg  (see  chapter  4  for  the  details).  In 
Proposition  (4,4),  we  demonstrate  that  these  paths  may  be  approximated  to  an 
arbitrary  resolution  by  successive  applications  of  the  local  operators,  with  only  a 
linear  increase  in  the  number  of  path  segments  as  the  resolution  grows  finer.  We 
have  also  found  it  useful  to  approximate  the  rotational  path  along  the  intersection 
as  follows. 

Given  two  level  C-surfaces  kcr  /  and  kcr  g  at  configuration  X,  we  wish  to 
choose  a  direction  from  X  tangent  to  both.  For  example,  if  the  configuration  space 
were  isomorphic  to  5R'*,  then  ker  /  and  kerg  would  both  be  two  dimensional  surfaces 
in  3-space,  and  this  direction  would  be  Vf{X}  X  Vg(X).  (Where  X  denotes  the 
standard  cross  product  on  3?^).  In  the  tangent  space  to  a  six-dimensional  C-Space, 
there  arc  typically  four  such  tangent  vectors  at  X  which  arc  tangent  to  kcr  /  and 
kerg.  We  will  demonstrate  an  operator  analogous  to  X  which  produces  one  such 
tangent  vector  in  a  natural  way.  (It  is  also  possible  to  solve  for  all  such  tangent 
vectors). 

We  begin  be  defining  an  extended  product  on  the  tangent  space  to  3?'^  X  50(3) 
at  X.  Let  V  =  (V’j;,V0)  G  Tx  be  a  tangent  vector  at  X.  We  may  think  of  Vj  and 
Ve  as  the  translational  an  I  rotational  components  of  a  six-dimcnsional  velocity 

'Although  we  also  employ  the  haekiiig  oil  heuristic  here. 


vector  V  at  X.  If  ly  =  6  Tx  is  another  tangent  vector  at  X,  we  dcGne 

the  extended  product  of  V  and  ly  by 


V  XW  =  {V^X  Vy„  Ve  X  We). 


The  cross  products  on  the  right  hand  side  arc  simply  the  standard  three-dimensional 
cross  product.  (Sec  below  (2.3)  for  why  this  makes  sense  for  the  rotational 
components,  Vq  X  Vye)-  If  y  =  V/  and  ly  =  Vg  then  V  X  ly  is  tangent  to  both 
ker  /  and  kerg  at  X.  Since  X  only  operates  on  tangent  vectors  to  X  SO(3) 
which  have  the  same  point  of  application,  we  will  never  have  reason  to  confuse  it 
with  X,  which  can  only  be  applied  to  three-dimensional  tangent  vectors. 

Let  f,g£A'  be  C-functions  generating  the  C-surfaces  ker  /  and  kerg  at  X. 
Observe  that  the  tangent  vector  V/(X)  X  Vg(X)  is  tangent  to  both  ker  /  and 
kerg  at  X.  We  can  locally  approximate  a  pure  rotational  trajectory  sliding  along 
the  intersection  of  /  and  g  by  a  path  in  direction 


7re(V/(X))  X  7re(Vg(X)).  (2.3) 

Note  that  this  is  well  defined  since 


TTo 
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^  ^  <21  ^  ^ 

dx'  dy'  dz'  dtp  ’  dO  ’  d^)  J 


df_  ^ 
dxl>'  dO'  d<t>)' 


The  (liffcrcntial  rotations  from  X  arc  isomorphic  to  a  three  dimensional  vector 
space,  and  hence  the  cross  product 


DO 


is  also  well  defined,  and  guaranteed  to  be  tangent  to  ker  /  and  kerg  at  X.  The 
Rotate  operator  can  be  called  in  succession  on  the  largest  components  of  (2.3) 
in  order  to  approximate  the  sliding  trajectory.  Of  course,  it  is  also  possible  to 
rc-evalnate  the  tangents  after  each  step. 


flr» 


2.4.5.  The  Sliding  Expert 

The  sliding  expert  attempts  to  find  a  path  sliding  along  one  level  C-surface 
at  X,  which  makes  progress  towards  the  goal.  The  sliding  expert  can  be  thought 
of  as  a  less  constrained  version  of  the  intersection  expert.  The  sliding  expert  tries 
to  choose  a  C-surface  in  A'  to  which  the  goal-direction  is  (almost)  tangent.  As  we 
will  see  in  chapter  4,  it  is  possible  to  choose  a  parameterization  along  a  C-surface 
which  maximizes  progress.  This  path  along  the  C-surface  can  then  be  realized  (at  a 
desired  resolution)  by  successive  applications  of  the  local  operators.  However  since 
there  are  many  paths  from  X  sliding  along  a  C-surface  at  X,  we  need  to  develop 
a  good  heuristic  strategy. 

Our  motivation  is  as  follows.  There  are  an  uncountable  number  of  paths  from 
X  sliding  along  a  C-surface  at  X.  We  could  maximize  a  directional  derivative  at 
X  to  choose  a  locally  optimal  search  direction.  This  would  work  once;  however, 
this  would  not  solve  the  problem  of  state:  it  is  necess.ary  to  partition  the  set  of 
paths  into  “neighborhoods,”  and  to  mark  a  neighborhood  of  paths  as  explored 
when  a  representative  from  that  neighborhood  is  selected  and  attempted  by  a  local 
operator.  In  principle,  a  computation  involving  homotopic  equivalence  classes  is 
possible  (see  Donald  (1983a)  and  appendix  111).  However,  this  requires  a  global 
computation  in  C-Space.  In  particubar,  the  image  of  all  paths  in  an  equivalence 
class  may  cover  X  50(3),  even  if  there  are  several  classes.  We  wish  to  find  a 
way  to  partition  the  paths  from  X  into  neighborhoods,  sample  a  canonical  element 
from  the  neighborhood,  and  evaluate  it  as  a  local  move  in  the  search. 

Given  a  C-surface  normal  V/  at  X,  we  wish  to  choose  a  direction  v  sliding  along 
the  C-surface  ker  /  which  maximizes  progress  to  a  subgoal.  Let  B  =  {x,y,z,ij),d,^) 
be  the  obvious  orthonormal  basis  for  the  tangent  space  to  X  50(3),  a.id 
-B  =  (~x,  -y,  -z,  —0,  -0,  -0). 

Next,  we  form  a  .set  of  vectors  orthogonal  to  V/(X)  as  follows: 


d  =  {v/(x)}0(bU-s) 


A 

where  ®  2  =  {p  X  g  j  p  G  /’i  9  G  }•  All  of  these  vectors  are  orthogonal  to 
ker  /  at  X.  We  then  choose  the  direction  v  ^  D  which  maximizes  ~  '^))i 

where  the  G  —  X  is  the  goal  direction.  If  is  the  heuristic  product  on  tangent 
vectors  inscad  of  the  single- valued  Riemannian  inner  product,  then  both  components 
of  the  image  of  should  be  maximized.  In  chapter  4  ,  we  will  see  that  it  is 
possible  to  comply  as  closely  as  desired  to  the  C-surface  ker  /  while  traveling  in 
direction  v. 

To  understand  this  strategy,  consider  the  following  example:  Suppose  we 
employ  a  basis  B'  which  only  spans  9?^.  Then  the  expert  will  choose  the  available 
translation  sliding  along  the  level  C-surface  which  maximizes  progress  towards  the 
goal.  Once  the  direction  v  is  chosen,  the  Translate  operator  is  invoked  to  slide 
along  the  level  C-surface  until  a  constraint  is  reached. 

There  is  no  need  for  the  basis  fl  to  be  orthogonal;  this  was  merely  adopted  for 
the  sake  of  intuitive  development.  The  basis  provides  a  sampling  of  the  function 
space  of  paths  compliant  to  the  C-surface  about  X. 

A  Conjecture  on  Completeness  using  Extended  Spanning  Sets 

By  using  the  basis  B,  we  obtain  a  12-way  sampling  of  the  space  of  directions 
orthogonal  to  V/  at  X — in  other  words,  there  are  12  vectors  in  D.  Imagine  using 
another  set  of  vectors,  B'*',  which  is  larger  than  B,  to  construct  D.  Then  D  would 
provide  a  finer  sample  of  the  space  of  directions,  since  more  directions  would  be 
sampled.  In  principle  it  should  be  possible  for  a  sample  to  be  complete  at  a  given 
resolution.  We  formalize  this  idea  as  follows: 

A  spanning  set  for  a  space  F  is  a  set  of  vectors  which  spans  V  yet  which  is 
not  necessarily  a  basis.  A  spanning  set  is  a  basis  for  V  which  has  been  extended 
by  adding  other  vectors.  We  conjecture  that  there  exist  certain  spanning  sets 
which  might  be  employed  to  construct  a  complete  planning  algorithm  without  the 
Bumble  strategy.  What  constitutes  such  a  complete  spanning  set?  The  analogue  of 
resolution  for  an  arbitrary  spanning  set  B'^  would  consist  in  (l)  the  cardinality  of 
the  spanning  set  and  (2)  the  uniformity  of  distribution  of  the  vectors 


about  the  unit  five- dimensional  sphere  S'*  in  the  tangent  space  at  X.  The  greater 
the  number  of  vectors  in  the  spanning  set,  and  the  more  uniform  their  distribution 
about  S^,  the  liner  the  resolution  of  the  planner.  The  development  of  such  a  planning 
algorithm  requires  surmounting  additional  theoretical  and  technical  dilliculties. 


2.4.6.  The  Rotation  Expert 


The  rotation  expert  is  built  on  the  rotational  operator  Rotate,  and  is  designed 
to  handle  some  of  the  special  problems  of  moving  through  rotation  space  that  are 
discussed  in  chapters  3,  4,  and  5.  The  rotation  expert  might  be  called  to  accomplish 
a  simple  rotational  subgoal,  or  in  conjunction  with  some  more  elaborate  strategy. 
In  particular,  when  a  translational  motion  terminates  by  striking  a  C-surface, 
forwarding  messages  are  left  for  both  the  sliding  expert  and  the  rotation  expert. 
The  former  has  been  discussed  as  the  “hit  and  slide”  strategy  (figure  2.4);  the  latter 
is  known  as  the  "hit  and  rotate”  technique  (figure  2.5). 

The  first  problem  that  the  rotation  expert  must  deal  with  is  the  “wrap  around” 
in  rotation  space.  A  subgoal  reached  in  directions  and  — although 

typically  one  is  “shorter”.  In  conjunction  with  the  planning  history,  the  rotation 


expert,  on  successive  applications  to  the  same  node,  can  develop  strategies  for 
rocking  back  and  forth  on  a  slice  of  rotation  space. 

The  Rotate  operator  is  more  constrained  than  the  Translate  operator  (in  that 
it  can  only  be  applied  in  ±i^,  ±0,  and  ±^).  Hence  the  rotation  expert  must  have 
a  method  for  approximating  rotational  trajectories  (specified  in  angle  space)  which 
arc  linear  combinations  of  the  rotational  basis  vectors,  such  as 

V  —  arp  -i-  bO  +  (2.4) 

for  some  scalars  a  ,  b,  and  c. 

In  terms  of  the  completeness  of  the  algorithm,  there  is  no  need  for  a  rotate 
operator  in  direction  (2.4)  (provided  a  path  along  v  lies  in  open  sets  of  free  space). 
In  chapter  4,  we  show  that  a  continuous  path  may  be  approximated  as  closely  as 
desired  by  a  sequence  of  moves  along  the  rotational  axes,  and  that  the  number 
of  staggered  path  segments  required  grows  only  linearly  as  the  resolution  becomes 
finer.  In  practice  this  use  of  the  restricted  rotate  operator  has  proved  adequate 
in  our  path-finding  experiments.  However,  it  is  heuristically  useful  to  realize  such 
paths  as  accurately  as  desired,  since  this  allows  higher  level  experts  to  suggest 
arbitrary  rotational  trajectories.  Given  such  a  trajectory,  the  rotational  directions 
are  ranked  by  magnitude  of  change,  and  the  unexplored  direction  of  greatest  change 
is  first  attempted.  On  failure,  or  upon  successive  applications  of  the  rotation  expert 
to  the  search  node,  the  other  directions  in  (2.4)  will  be  attempted.  This  process 
leads  to  the  approximation  of  arbitrary  pure  rotations  by  a  staggered  sequence  of 
rotations  along  the  axes.  If  the  extent  of  each  rotation  is  limited,  the  approximation 
can  be  made  arbitrarily  fine.  To  approximate  motion  in  a  direction  such  as  (2.4),  the 
planner  actually  attempts  several  of  the  directions  simultaneously,  which  results  in 
a  spanning  “box”  of  rotational  moves  about  the  idealized  trajectory  (in  the  absence 
of  obstacles). 

Suppose  a,  6,  and  c  in  the  idealized  trajectory  (2.4)  arc  positive.  This  yields 
a  set  of  positive,  or  “forward”  rotational  directions,  and  a  set  of  “backwards” 
rotational  directions  which  can  attain  the  goal.  Which  directions  are  forward  and 


which  are  backward  depend  upon  the  distance  (in  the  vector  parameter  space  V) 
of  the  goal  from  X ,  that  is,  on  7re(G  —  For  example,  if  is  negative  and 

small,  then  will  be  a  backwards  direction,  and  — ^  will  be  a  forward  direction. 

The  rotation  expert  develops  and  ranks  these  sets  of  forward  and  backward 
rotational  directions.  By  examining  the  planning  history  and  the  local  geometry  of 
C-surfaces  at  X,  these  sets  of  directions  are  in  turn  pruned.  In  particular,  local 
C-surfaces  that  would  block  a  particular  rotational  motion  are  detected.  For  a 
direction  v,  this  is  done  by  examining  the  magnitude  of  the  directional  derivative 
in  V.  The  importance  of  such  an  impediment  is  then  heuristically  ranked  by  the 
closeness  of  the  C-surface  at  X.  Special  consideration  is  given  to  C-surfaces  which 
have  a  history  of  proving  troublesome.  For  example,  when  an  expert  runs  into  a 
C-surface,  the  reason  for  stopping  is  left  as  part  of  the  move  explanation.  If  the 
rotation  expert  is  invoked  as  part  of  a  “hit  and  rotate”  strategy,  then  we  must 
ensure  that  the  planner  tries  to  rotate  away  from  the  C-surface(s)  which  blocked 
progre.ss.  The  rotational  directions  which  point  away  from  C-snrfaeea  may  he  found 
by  examining  V /.  The  process  of  determining  the  rotational  constraints  from  the 
local  geometry  of  C-surfaces  is  closely  related  to  our  earlier  discussion  of  detecting 
rotationally  orthogonal  C-surfaces. 

Thus  the  requested  rotational  trajectory  and  rotational  goal  provide  a  set 
of  desired  rotational  motions.  The  planning  history  supplies  a  set  of  rotational 
constraints,  and  from  the  local  C-surface  geometry  can  be  inferred  a  set  of 
preferred  and  prohibited  motions.  The  constraints,  preferences,  and  prohibitions 
arc  intersected  with  the  forward  and  backward  desires.  This  yields  a  set  of  rotational 
directions  which  will  be  attempted  using  the  Rotate  operator.  Depending  on  the 
kind  of  invocation,  the  rotation  expert  may  apply  the  Rotate  operator  up  t'' 
some  fixed  number  of  times-  -this  is  particularly  useful  when  it  must  attempt  to 
approximate  an  idealized  rotational  trajectory  which  is  a  linear  combination  of  the 
basic  rotational  directions. 

Canny  (IQS'!)  has  recently  extended  the  Rotate  operator  for  directions  such  as 
eq.  (2.4),  corresponding  to  uniform  rotation. 
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2.1.7.  The  Around  Expert 


The  around  expert  attempts  to  circumnavigate  obstacles  by  sliding  around 
their  boundary.  An  idealized  illustration  of  the  around  expert  is  shown  in  figure  2.6. 
The  around  expert  is  similar  to  the  sliding  expert,  except  that  instead  of  attempting 
to  find  a  C-surface  which  contains  a  path  towards  the  goal,  the  around  expert 
searches  for  a  C-surface  which  is  (roughly)  locally  orthogonal  to  the  goal  direction. 
Next  a  path  is  planned  sliding  along  this  surface  in  a  direction  v'  orthogonal  to  the 
goal  direction;  the  path  is  attempted  using  a  local  operator.  Typically,  this  motion 
will  result  in  a  search  node  s'  which  is  farther  from  the  goal  than  the  parent  node, 
s.  Ordinarily,  s'  would  not  be  explored  soon,  since  other  search  nodes  would  appear 
more  promising  to  the  planner’s  best-first  strategy.  In  order  to  give  the  around 
strategy  a  chance,  the  around  expert  explicitly  places  s'  at  the  front  of  the  search 
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queue  and  calls  the  planner  recursively. 

The  around  expert  can  also  invoke  the  intersection  expert.  Recall  that  the 
intersection  expert  normally  tries  to  construct  tangent  intersection  manifolds  which 
contain  paths  towards  the  goal.  However,  when  called  from  the  around  strategy, 
it  can  construct  intersection  manifolds  locally  orthogonal  to  the  goal  direction.  To 
construct  the  intersection  set  of  locally  orthogonal  level  C-rnanifolds,  we  perform  a 
pairwise  intersection  of  C-manifolds  locally  orthogonal  to  the  goal  direction  at  X . 

2.4.8.  The  Suggeator 

The  suggestor  is  a  strategy  for  proposing  good  subgoals  in  configuration  space. 
As  we  saw  in  Donald  (1983a),  one  of  the  problems  with  local  operators  even  if 
they  are  complete  (that  is,  their  closure  covers  configuration  space),  is  that  without 
good  subgoals,  they  may  take  a  long  time  to  converge.  The  suggestor  is  a  heuristic 
strategy  for  setting  subgoals  in  C-Space. 

First,  a  very  coarse  lattice  is  thrown  over  C-Space.  This  lattice  is  then 
searched  for  a  sequence  Q  of  free  configurations  [not  a  path)  stepping  through  the 
lattice  to  the  goal.  If  no  such  sequence  can  be  found,  then  configurations  on  a 
promising  partial  sequence  are  employed.  These  configurations  may  then  be  set  a.s 
subgoals,  and  the  planner  can  be  called  recursively.  The  configurations  Q  represent 
intermediate  planning  islands  of  safe  configurations.  If  paths  can  be  found  between 
these  configurations,  then  the  find-path  problem  is  solved.  Otherwise,  expanding 
from  any  partial  paths  found  can  also  prove  useful,  in  that  the  planning  islands 
effectively  distribute  the  application  of  local  experts  and  operators  over  more  of 
configuration  space. 

The  suggestor  complicates  the  connectivity  of  the  explored  neighborhoods 
graph.  The  ability  to  explore  arbitrary  subgoals  and  suggested  paths  requires 
more  complicated  bookkeeping  for  neighborhood  exploration:  we  must  employ  the 
connect  strategy,  in  order  to  know  when  partial  paths  link  up.  If  partial  paths 
not  rooted  at  the  start  neighborhood  are  permitted,  then  the  graph  of  explored 
neighborhoods  will  not  necessarily  be  connected,  and  the  mark  strategy  will  fail  (the 
mark  strategy  constructs  a  directed,  spanning  tree  for  a  connected,  rooted  graph 


of  explored  neighborhoods).  Happily  the  connect  strategy  will  succeed,  since  it  is 
defined  on  an  arbitrary  graph.  An  algorithm  for  the  connect  strategy  is  discussed 
in  section  2.1.2. 


Listing  I:  The  log  of  expert  explanations  for  thr  path  in  ligure  2.7. 


(find-path  ‘si  *01) 

Verifying  the  start  and  goal  points... 

start  :  (0  0  0  1  1  11)  ,  goal  :  (-6  10  0  0  0  0)  . 

Starting  search,  boss... 

Exploring  (0  0  0  1  1  11)  .  .  . 

Local  Expert:  I  translated  straight  towards  goal,  reaching  ((-1  1011  11)) 
Exploring  (-1  1011  11) . . . 

Local  Expert:  I  Slid  along  a  level  C-Manifold,  reaching  ((-6  1011  11)) 
Exploring  (-6  10  11  11)  .  .  , 

Local  Expert:  I  translated  straight  towards  goal,  reaching  ((-6  10  0  1  1  11)) 
Exploring  (-6  10  0  1  1  11)  .  .  . 

Rotation-Export:  Found  0  guiding  constraints  on  rotational  motion. 
Rotation-Export:  Intersected  Rotational  Constraints  with  desired 
rotations  yielding  posslblo  motions  in 
((MINUS  PHI)  (MINUS  PSI)  THETA). 

Rotation-Expert:  I  am  trying  to  rotate  in  (PLUS  THETA)  ... 

Local  Expert:  I  rotated  to  reach  ((-6  10  0  1  1  0)) 


Exploring  (-6  10  0  1  1  0)  .  .  . 

Rotation-Export:  Found  0  guiding  constraints  on  rotational  motion. 

Rotation-Export:  Intersected  Rotational  Constraints  with  desired 
rotations  yielding  possible  motions  in 
((MINUS  PHI)  (MINUS  PSI)). 

Rotation-Export:  I  am  trying  to  rotate  in  (MINUS  PHI)  ... 

Local  Expert:  I  rotated  to  roach  ((-6  10  0  0  1  0)) 

Exploring  (-6  10  0  0  1  0) . . . 

Rotation-Export:  Found  0  guiding  constraints  on  rotational  motion. 

Rotation-Export:  Intorsoctod  Rotational  Constraints  with  desired 
rotations  yielding  possible  motions  in 
((MINUS  PSD). 

Rotation-Expert:  I  am  trying  to  rotate  in  (MINUS  PSI)  ... 

Local  Expert:  I  rotated  to  reach  ((-6  10  0  0  0  0)) 

Exploring  (-6  10  0  0  0  0) . . . 

[success!]  Saving  and  Drawing  final  path... 

Dack  to  Lisp  Top  Level  in  Lisp  Listener  2 
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Figure  2.12.  View  II:  (frame  19),  The  final  configuration. 

2.5.  Examples  of  the  Local  Experts  in  Use 

In  6gure  2.7,  we  show  a  very  simple  example  of  a  path  found  using  local 
experts.  Listing  I  shows  a  log  of  the  expert  explanations  for  each  move. 

The  “Thor’s  Hammer”  example  in  chapter  1  was  produced  by  disabling  all 
experts,  and  employing  only  the  Bumble  strategy.  (Please  refer  to  this  figure).  In 
the  accompanying  figures  (2.8-13),  we  show  a  path  found  by  a  strategy  comprising 
all  the  experts  described  above.  The  solution  path  is  very  different,  and  tends  to 
slide  around  obstacles  instead  of  finding  convoluted  paths  between  them. 

Figures  2.14-21  show  the  solution  for  a  find-path  problem  in  a  cartesian 
workspace.  A  cartesian  workspace  is  a  bounding  box  beyond  which  the  reference 
point  may  not  translate.  However,  the  bounding  box  imposes  no  restrictions  on 
rotations.  The  Movers’  problem  in  a  cartesian  workspace  is  similar  to  the  motion¬ 
planning  problem  for  cartesian  manipulators,  and  the  L-shaped  object  may  be 
thought  of  as  the  (wrist  and)  payload.  First,  we  show  the  reference  point  on 
the  L-shaped  object.  Next  two  views  are  presented  of  the  path  found  within  the 
workspace,  around  a  large,  diagonally-placed  obstacle.  View  (II)  is  a  view  from 
the  side;  view  (I)  is  a  view  from  the  top.  Only  the  back  faces  of  the  rcctangloid 
workspace  arc  shown.  Since  the  rotation  from  frames  13  to  14  is  very  large  (> 
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Figure  2.8.  View  I;  (frames  ]  9).  Tlie.sc  18  frames  show  a  solulion  path  for  the  “Thor’s  Uaminer" 
Mover’s  problem.  Loral  experts  (as  dj-scribed  in  this  chapter)  are  employed  to  slide  the  moving 
oliject  along  level  C- Manifolds.  Three  views  arc  shown.  The  final  configuration  is  only  visible  in 
view  II  (figure  2.12). 


Figure  2.11.  View  II:  (frames  10  18). 


no 


in  the  —V’  direction),  a  detail  of  the  rotation  is  also  shown. 

2.6.  Path  Planning  versus  Discrete  Intersection  Detection 

Imagine  a  brute-force  planner  which  discretizes  configuration  space,  places  the 
robot  at  every  point  in  the  discretization,  and  tests  for  intersection.  This  would 
yield  a  discrete  set  of  configurations  where  the  robot  could  be  placed.  Alternatively, 
the  tests  could  be  structured  in  a  search.  As  stated  so  far,  this  is  not  collision-free 
path  planning.  Path  planning  ensures  that  a  path  exists  between  each  configuration 
on  the  path.  It  has  been  argued  that  if  the  intersection-detection  is  done  at  a 
fine  enough  resolution  then  a  path  will  have  been  cfTectively  found.  At  a  given 
resolution,  it  is  possible  to  bound  the  size  of  the  intersection  between  the  robot 
and  any  obstacle  which  can  occur  between  intersection  checks.  This  bound  grows 
smaller  as  the  sampling  of  the  space  grows  finer.  By  growing  the  real-space  obstacles 
by  this  bound,  it  is  possible  to  ensure  that  no  collisions  occur  between  discrete 


t]  (Vl»w  II.  PtobUm  7) 


Figure  2.21.  Detail  of  the  rotation  from  frames  13-14. 

intersection  checks  (Gouzcncs  (1983)).  Of  course,  if  the  resolution  is  insufficiently 
fine,  then  the  obstacles  may  be  grown  so  much  that  no  path  can  be  found. 


For  gross  motion  planning  in  an  uncluttered  environment,  this  approximate 
method  may  perform  reasonably  well.  In  complicated  environments,  however,  the 
resolution  will  have  to  be  fine  in  order  to  ensure  that  paths  are  collision  free 
without  growing  the  obstacles  so  much  that  no  path  can  be  found.  We  will 
compare  the  asymptotic  complexity  of  the  discrete  intersection  method  with  the 
Rotate  operator.  (The  Rotate  operator  is  the  most  complex  local  operator).  The 
fundamental  observation  is  that  the  complexity  of  the  discrete  intersection  method 
varies  linearly  with  the  sampling  resolution,  whereas  the  complexity  of  the  Rotate 
operator  is  independent  of  (any)  resolution.  This  is  because  our  discretization  is 
quite  different:  a  lattice  is  thrown  on  the  space  in  order  to  record  the  state  of  the 
planner  and  the  connectivity  of  the  explored  neighborhoods. 
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Consider  the  following.  Suppose  X  and  Y  are  configurations  on  a  rotational 
trajectory  in  direction  Suppose  further  that  the  robot  is  composed  of  m  convex 
polyhedra  containing  k  generators  each,  and  that  there  are  n  convex  obstacles 
containing  j  generators  each.  The  number  of  faces  on  a  robot  polyhedron  or  an 
obstacle  polyhedron  is  0(/c)  (respectively,  0[j)).  To  perform  one  intersection  check 
(at  a  single  configuration)  for  one  robot  polyhedron  and  one  obstacle  polyhedron 
requires  time  0[\og^[j  +  k))  (Dobkin  and  Kirkpatrick  (1980)).  This  theoretical 
intersection  algorithm  has  not  yet  been  implemented,  but  we  consider  it  since  it 
is  the  fastest  known.  To  perform  a  check  (at  one  configuration)  for  the  entire 
robot  against  the  entire  obstacle  environment  requires  tinie  0(mn  log‘*(y -f  A:)).  Now 
suppose  that  the  path  segment  [X,Y\  must  be  sampled  i  times  for  the  quantizing 
intersection  checker.  This  requires  time 


0(imn  log^(j  +  A:)). 

In  chapter  5,  we  show  that  our  planner’s  Rotate  operator  could  determine  whether 
there  exists  a  path  from  X  to  K  in  time  0{N  log  N)  (where  N  is  the  number  of 
C-surfaces).  In  chapter  3,  we  show  that  N  =  0[mnjk).  Hence  the  complexity  for 
Rotate  is 


0{mnjk\og{mnjk))  —  0{mnjk{\ogTn  +  logn  +  logj  +  log  A:)). 

Rotate  ensures  tliat  there  exists  a  path  from  X  to  Y  without  growing  the  real-space 
obstacles,  and  does  not  involve  a  resolution  factor  i.  Holding  k  and  j  fixed,  the 
relative  asymptotic  performance  of  the  quantizing  intersection  detector  and  the 
Rotate  operator  will  depend  on  whether  or  not 


i  >  log(mn). 


The  constants  i,  j,  and  k  will  depend  on  particular  workspaces  and  find-path 
pjroblcms.  However,  we  believe  that  in  order  to  be  reasonably  sure  of  the  safeness 


of  a  path  between  configurations  without  growing  the  real-space  obstacles  too 
much,  i  may  have  to  be  quite  large.  This  is  especially  true  in  reasonable  sized 
environments.  So  as  mn  increases,  the  workspace  becomes  more  crowded  and/or  the 
robot  becomes  more  complicated,  and  the  sampling  rate  will  have  to  be  increased. 
We  think  it  unlikely  that  the  sampling  rate  will  grow  only  logarithmically  with 
the  workspace  complexity.  Moreover,  the  theoretical  0{\og^{j  +  k))  intersection 
time  for  the  Dobkin  and  Kirkpatrick  (1980)  algorithm  assumes  that  the  solid 
models  of  the  m  robot  polyhedra  are  precomputed.  (If  the  solid  models  must  be 
computed  for  each  configuration,  then  this  will  take  0{km)  additional  time  per 
sample  point).  In  addition,  for  a  polyhedron  with  k  faces,  0(fclogA:)  preprocessing 
time  is  required  by  the  algorithm  (for  each  intersection  check),  which  would 
yield  an  even  higher  complexity  for  the  discrete  path  planning  algorithm.  At  this 
stage,  since  the  algorithm  is  unimplemented,  it  is  unclear  whether  some  sort  of 
lazy  evaluation,  parametric  representation,  or  cITicient  precomputation  could  be 
employed  to  reduce  the  complexity  of  iterative  application  of  this  intersection  test. 
Most  implemented  intersection  detectors  that  arc  reasonably  robust  have  time 
complexity  0{{j  +  k)  log(j  -f  k))  or  0((;  +  k)"^).  However,  it  is  possible  to  employ 
minimum  distance  checks,  or  0{j  -f  k)  intersection  checks  in  some  cases. 

Summary 

In  a  practical  planning  system,  there  are,  of  course,  other  considerations.  For 
example,  our  employment  of  the  Rotate  operator  requires  time  to  update  the  lattice. 
The  main  point  is  as  follows:  on  a  lattice  of  spacing  d,  to  verify  the  safeness  of  a 
path  of  length  di,  the  discrete  intersection  method  requires  at  lc;ist  time  O(zmn), 
whereas  the  Rotate  oper.ator  requires  time  0{mn]ng{mn)).  The  discrete  method 
actually  does  not  ensure  safeness,  but  merely  that  the  intersection  “size”  is  no 
greater  than  some  function  of  d. 

CompcttMicn  versus  Performance 

We  have  shown  that  the  relative  performance  of  the  two  algorithms  will  largely 
depend  on  the  constants  in  the  j)roblem.  For  gross  motion  in  uncluttered  workspaces 
the  discrete  intersection  algorithm  will  probably  perform  better.  In  complicated, 
crowded  environments,  or  in  problems  requiring  motions  close  to  the  obstacles,  the 


required  sampling  rate  will  probably  be  prohibitive.  In  addition  to  the  question 
of  [)crformance,  we  should  also  mention  the  issue  of  competence.  (In  linguistics, 
competence  refers  to  the  knowledge  base,  and  performance  refers  to  how  well  it  is 
used).  The  representations  we  develop  in  subsequent  chapters  arc  applicable  not 
only  to  the  find-path  problem  with  six  degrees  of  freedom,  but  also  to  the  class  of 
geometric  planning  problems  described  in  chapter  1  (for  example,  fine  motion,  and 
planning  with  uncertainty).  It  is  clear  from  previous  work  that  these  problems  are 
within  the  competence  of  the  representation  we  develop  for  5)?'^  X  ^>’0(3)  (Mason 
(1981),  Lozano-Perez,  Mason,  and  Taylor  (1983),  Erdmann  (1984)).  At  this  point  we 
have  no  indication  that  these  problems  are  within  the  competence  of  the  discrete 
intersection  method.  (Find-space,  however,  can  be  accomi)lishcd  using  discrete 
intersections). 
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Questions  of  Representation:  C-functions  and  Applicability 
ConstraiiTts  in  a  Six  Dimensional  Configuration  Space 

In  this  chapter,  we  first  present  a  formal  framework  in  which  several  open 
questions  about  configuration  space  constraints  may  be  resolved.  This  framework 
has  been  discussed  informally  in  the  first  two  chapters.  Wc  then  proceed  to  construct 
and  prove  a  set  of  theorems  about  the  domains  and  domain  topology  of  C-functions 
for  the  classical  Movers’  problem  with  six  degrees  of  freedom. 

These  theorems  allow  us  to  ilefine  the  applicability  constraints  on  C-functions  for 
the  Movers’  problem  in  3?'*  X  SO(3).  Every  C-function  characterizes  a  constraint  on 
motion  only  within  a  certain  region  of  rotation  space.  Determining  what  constraints 
are  applicable  at  a  given  orientation  (or  range  of  orientations)  is  of  fundamental 
importance  to  the  mathematical  framework  for  the  spatial  planning  problem;  in 
order  io  plan  using  constraints,  we.  must  know  where  (at  what  orientations)  these 
const  mints  are  applicable.  Recall  that  each  C-function  is  generated  by  a  pair  of 
boundary  cells  (a,  6),  where  a  lies  on  the  boundary  of  a  moving  polyhedron  and  6  on 
the  boundary  of  an  obstacle  polyhedron.  Put  simply,  the  applicability  constraints 
determine  what  boundary  cells  a  and  b  can  interact  at  a  given  orientation. 

3.1.  Definitions  and  Conventions 

Let  A  denote  any  rigid,  convex  set.  >1(0)  denotes  A  rotated  to  orientation  0. 
Formally,  if  0  is  an  orientation,  and  .C(0)  is  the  corresponding  rotation  operator. 


t  hen  .4((-))  denotes  -C(0)  applied  to  A.  As  a  kind  of  shorthand,  we  refer  to  /\(0)  as  “A 
at  orientation  0,”  or  “A  rotated  to  orientation  0.”  I'or  example,  if  F  is  a  face,  then 
/''(0)  denotes  F  at  orientation  0.  /'”s  normal,  N,  rotates  witli  F,  and  is  denoted 
A’(0).  We  assume  face  normals  are  outward-directed  from  the  polyhedra  they 
bound.  We  will  in  general  use  A  to  denote  a  convex  moving  polyhedron,  and  F  for  a 
convex  obstacle  polyhedron.  If  e„  is  an  edge  of  A  an<I  mid(<’„)  denotes  its  midpoint, 
then  mid(e„(0))  denotes  its  midpoint  at  orientation  0.  At  this  point  it  is  not 
convenient  to  commit  ourselves  to  any  particular  repre.sentation  for  3-dimensional 
rotations.  However,  the  rcailer  may  without  esseiitial  loss  of  generality  interjrret 
r;(0)  (for  v  G  as  the  rotation  matrix  -5(0)  applied  to  the  vector  v,  where  /?(0) 
might  be  parameterized  by  ICuler  Angles.  Since  ^(0)  is  an  orthonormal  matrix, 
[.^(0)]  '  — =  can  be  employed  to  rotate  a  plane  which  is  represented  as  a 

'1-dimcnsional  vector.  This  operation  yields  the  rotated  normal  /V(0)  of  course  (sec 
Paul  (1981)).  However,  note  that  the  results  of  this  chapter  are  independent  of  any 
particular  representation  of  rotations,  and  that  -?(0)  is  properly  a  generic  rotation 
operator,  u  •  v  denotes  the  standard  inner  product  on  of  u  and  v.  If  u  and  v  are 
complicated  expressions,  however,  we  will  use  the  notation  (u,v). 

The  six  dimensional  configuration  space  9?'^  X  50(3)  is  formally  defined  in 
chapter  2.  X  will  denote  a  configuration  in  this  space.  We  will  identify  0  with 
.5(0)  and  write  0  G  5'0(3).  Writing  X  —  (i,  0)  niakes  explicit  the  translational 
component  of  the  configuration  (a:)  and  the  rotational  component  (0  or  -C(0)). 

d  denotes  the  boundary  operator.  For  example,  if  F  is  a  face  on  a  polyhedron 
n,  then  OF  denotes  the  ring  of  edges  which  bound  F.  dU  denotes  the  faces  of  li, 
de  for  an  edge  e  denotes  e’s  vertices,  and  so  forth.  The  coboundury  operator  is  the 
dual  of  the  boundary  operator  and  is  denoted  S.  The  coboundary  of  a  vertex  is  the 
set  of  edges  incident  there;  the  coboundary  of  an  edge  arc  the  faces  which  the  edge 
bounds;  and  the  coboundary  of  a  face  is  the  zero,  one,  or  two  solids  it  bounds.  In 
chapter  5,  we  provide  a  formal  definition  of  bounrlary  and  coboundary  using  the 
chain  groups;  alternatively,  see  Hocking  and  Young  (1961)  or  Ciblin  (1977). 

We  denote  the  faces,  edges,  and  vertices  of  a  polyhedron  H  by  fac.es[Ii), 
edges(£i),  and  vcrt{li),  respectively. 


If  S  is  a  set  then  z(S)  denotes  its  interior,  and  kS  its  closure.  kS  —  t(5)U^-5. 
We  denote  the  classical  Movers’  problem  with  six  degrees  of  freedom  by  &DOF. 

3.2.  Representing  Constraints  in  Cunliguratiuti  Space 

Lozano-Perez  (1983)  showed  that  the  C-Space  obstacles  can  be  represented  as 
an  intersection  of  a  finite  number  of  half-hypersp.aces,’  where  each  half-hyperspace 
is  represented  via  a  constraint  function  of  the  form 


/.  :  5R'*  X  50(3)  —  S9 

where  the  sign  of  /,(A')  determines  whether  X  is  inside,  on,  or  outside  the  C-Space 
obstacles.  However,  when  rotations  are  allowed,  each  constraint  function  is  valid, 
or  applicable  only  within  a  certain  region  Ai  of  the  rotation  space: 

/.  :  X  A  -  5R  {Ai  C  50(3)). 

We  call  such  a  function  /,  a  C-function.  We  consider  the  robot  and  obstacles  to 
be  modeled  by  the  (possibly  overlapping)  union  of  convex  polyhcdra,  and  define 
a  boundary  cell  to  be  a  face,  edge,  or  vertex  of  such  a  polyhedron.  C-functions 
model  constraints  on  motion  generated  by  pairs  of  cells  (9,,,  g/,)  where  ga  .and  g/,  are 
boundary  cells  on  the  robot  and  on  an  obstacle,  respectively.  Lozano-Perez  (1983) 
identified  throe  types  of  interactions:  (face, vertex),  (vertex, face),  and  (edge, edge), 
which  to  preserve  tradition  we  shall  term  type  (a),  (b),  and  (c)  constraints.  However, 
these  interactions  can  only  occur  in  certain  orientations;  for  example,  it  is  easily  seen 
that  although  two  cuboids  generate  LH  type  (c)  ct)nstr.aints,  at  any  fixed  orientation 
only  certain  edges  can  interact  and  hence  only  certain  type  (c)  constraints  are 
applicable.  The  region  of  rotation  space  where  a  C-function  /,  is  .applicable  is  it’s 
.applicability  region,  Ai-  The  domain  of  /,,  then,  is  9?'*  X  Ai- 

For  the  two-dimensional  Movers’  problem,  the  rotation  sp<ace  is  the  l-sphere 
and  the  applic,ability  regions  Ai  are  simply  sectors  on  5*.  While  Lozano-Perez 
'For  a  moving  object  and  ol)Mlacle»  o'presn’nted  as  uvorlappiiig  unions  of  convex  polyhcdra. 
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(1983)  was  able  to  (Icliiio  the  I’orin  of  C-Space  coiislraiiils  /,  for  (iDOF,  previous 
work  has  not  been  able  to  fortnulate  the  applicability  regions  in  i)0(3). 

We  begin  by  defining  CO  C  9?*  X  .90(3),  the  space  of  forbidden  configurations: 

oo  =  {xiVf^«W}  (1) 

a 

where  C,,  is  a  constraint  sentence  (sec  Brooks  and  l^ozano-f’ercz  (1983)).  a  is  indexed 
by  C- Space  obstacles.  For  each  C-Space  obstacle  0„,  O,,  maps  a  configuration  X 
to  true  or  false,  depending  on  whether  X  is  inside  Oa-  (1)  states  that  if  X  is  inside 
any  C-Space  obstacle,  then  it  is  in  CO. 

For  X  =  (i,  0), 

C^{x,  0)  =  A(e  €  Ai  =>  /.(x,  0)  <  o).  (2) 

Let  us  parse  (2).  The  index  i  ranges  over  the  set  of  all  C-functions  {/i,--.,/n} 
which  define  the  C-Space  obstacle  Oa-  We  call  such  a  set  of  C-functions  a  family 
of  C-functions.  This  family  is  generated  by  considering  pairwise  interactions  of 
features  on  the  boundary  of  A  and  features  on  the  boundary  of  B,  where  A  is  a 
convex  polyhedron  on  the  moving  object,  and  D  \s  a,  convex  obstacle  polyhedron. 
For  a  two  dimensional  example,  refer  to  figures  1.29-32  (chapter  1),  which  illustrate 
an  obstacle  polygon  B  with  four  vertices,  and  a  moving  polygon  A  with  three 
vertices.  For  these  two  polygons,  the  family  of  constraints  generated  corresponds 
to  all  possible  interactions  of  their  edges  and  vertices: 

f  amily2i){A,  B)  =  ^faces{A]  X  ver  J(f7)j  (J^ucr<(A)  X  faces{B)^ 

=  ({ei,e2,e;,}  X  U({  }  X  {e{,e^,e^,  } 

Each  pairing,  for  example  (ei,t;{),  generates  exactly  one  C-function  /,-.  In  three 
dimensions,  a  family  of  C-functions  corresponds  to  a  set  of  constraints  resulting 
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from  the  possible  interactions  of  one  polyhedral  component  of  the  moving  object, 
and  one  obstacle  polyhedron: 


f amily{i)[A,  B)  =  aces{^A)Xver  t{B)^  |J^efi!7es(yl)  Xedges(/i)^. 

Of  course,  in  both  two  and  three  dimensions,  at  a  given  orientation,  only  a  subset  of 
this  family  is  applicable.  For  each  C-function  /,,  there  is  an  associated  applicability 
region  Ai-  Equation  (2)  for  C,,  can  be  parsed  as  follows:  for  a  configuration  X,  for 
each  C-function  /,  such  that  X  is  in  the  domain  of  /,,  fi{X)  must  be  negative-valued 
(or  zero)  for  X  to  be  inside  the  C-Space  obstacle  Oa-  To  determine  whether  X 
is  in  the  domain  of  /,,  test  whether  the  rotational  component  of  X  is  within  the 
applicability  region  Ai- 

Next,  we  define 

F  =  X  50(3)  -  CO 
to  be  the  space  of  free  configurations. 

Now,  for  each  C-function  /,,  Ai  C  50(3)  is  the  corresponding  portion  of 
rotation  space  where  /,  is  applicable.  We  construct  Ai  as  the  intersection  of  a  set 
of  half-hyperspaces  on  50(3): 


yl.  =  {0€5O(3)|A(M©)>o)} 


where  gj  :  50(3)  — +  3?  is  an  npplicabilily  constraint  function  {ACF).  A  C-function 
fi  is  said  to  be  applicable  for  a  configuration  X  —  (x,0)  if  0  C  Ai-  In  this  chapter, 
we  will  derive,  and  prove,  the  form  of  the  ACFs.  Geometrically,  the  applicability 
regions  Ai  are  complicated  three  dimensional  manifolds  (with  boundary)  on  the 
projective  3-spherc.  Their  boundaries  are  the  two  dimensional  manifolds  kcr<?y.  (j 
indexes  over  the  set  of  functions  used  to  construct  Ai.  There  are  typically  three  or 
four  gj,  as  we  will  see  later). 
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The  form  of  the  applicability  constraints  was  heretofore  unknown.  Many  of 
the  representational  and  algorithinic  issues  for  geometric  planning  problems  with 
six  degrees  of  freedom  rely  on  a  correct  formulation  of  the  applicability  constraints. 
With  these  advances,  however,  the  mathematical  framework  will  be  com|)lctc,  and 
we  can  construct  the  planner  of  chapter  2  which  exploits  the  geometry. 

The  work  of  Brooks  and  Lozano- Perez  (1983)  dealt  with  surfaces  in  the  C-Space 
X  5',  which  arc  called  C- surfaces.  The  obvious  extension  of  this  concept  for 
GDOF  is  a  C- manifold  in  9?'*  X  50(3).  For  a  C-function  /,  vve  ilefinc  a  level 
C-manifold  to  be  the  set  of  configurations  X  where  /,  is  applicable  and  fi{X)  =  £, 
for  some  level  t.  Thus  a  level  C-manifold  is  the  level  set  /,~'(^).  Of  particular 
interest  is  the  C-manifold 

kcr/.  =  /,  ‘(0)  =  {X1/.(X)  =  0}, 

which  contains  a  boundary  patch  of  a  C-Space  obstacle.  Since  in  the  literature, 
C-manifolds  of  this  form  have  been  called  C-surfaccs,  wo  shall  also  employ  this 
term. 

We  now  define  paths  in  C-Space.  Given  a  start  configuration  s  and  a  desired 
goal  configuration  (j,  a  successfid  collision-free  path  is  a  continuous  function 
p  ;  7’  9i'’  X  50(3)  such  that  p(0)  =  s,  p(l)  --  g,  and  7>(f')  C  I'  ■  denotes  the 

closed  unit  interval,  [0,  1]. 

3.3.  Tlu!  Geometric  Interpretation  for  C-finictions 

Consid(!r  the  interaction  of  an  obstacle  polyhedron  D  and  a  moving  polyhedron 
A,  where  both  A  and  I)  are  convex.  Let  fp  be  in  the  family  of  C-functions  generated 
for  A  and  If.  fp  models  a  constraint  on  the  motion  of  A.  T'or  example,  fp  might  be 
generated  by  considering  the  interaction  of  a  face  of  A  and  a  vertex  of  13.  For  a 
given  orientation  0,  the  projection  into  9?'*  of  any  (applicable)  C-manifold  fp  '(O) 
is  a  plane  corresponding  to  a  face  of  the  polyhedron  resulting  from  the  Minkowski 
sum  of  (^A  and  II,  that  is. 


/yO^{Q)  =-  {6  +  a(0)  |6€  Z^,a€0A} 


where  a(0)  denotes  vector  a  rotated  to  orientation  0  and  |  a  6  }. 

(Note  that  in  constructing  “negation”  takes  place  before  the  rotation). 

1)  O  /1(0)  is  the  projection  into  3?'*  of  the  C-Spacc  obstacle  at  orientation  0.  In 
effect,  wc  liave  parameterized  the  plane  equations  of  faces  of  B  Q  A[G)  by  0.  Here 
is  the  form  of  the  parameterized  plane  equations  derived  by  Lozano- Perez  (1983); 
0,(0)  is  a  vertex  of  Ozt(0)  and  bj  is  a  vertex  of  IJ.  Itecall  that  the  equation  of 
a  plane  in  9?*  can  be  expressed  as  {  i  |  {N,x)  =  {N,q)},  where  N  is  the  plane 
normal  and  q  is  a  reference  point  known  to  be  on  the  plane.  Then  C-functions  take 
the  form: 


f„ix,  0)  -  (/V(0),  x)  -  {N{e),  (0,(0)  +  b,))  (4) 

where  x  is  a  point  in  D?'*.  N{Q)  is  the  real-space  component  of  the  C-manifold 
normal  at  orientation  0,  and  is  defined  as  follows:  for  a  type  (.a)  C-function, 
N{€>)  is  the  normal  of  a  face  of  0/1(0).  For  a  type  (b)  C-funcLion,  N{Q)  is  the 
normal  of  a  face  on  D,  and  hence  is  constant.  For  a  type  (c)  C-function,  N{Q)  is 
the  cross-product  of  an  edge  on  B  and  an  edge  on  0y\(0).  Furthermore  N[&)  is 
normalized  to  a  unit  vector  when  it  is  non-zero. 

The  geometric  significance  of  /j,(i,0),  is  now  clear.  The  value  of  fp  represents 
how  far  the  (reference)  point  i  lies  above  the  plane  of  a  face  in  the  0-parametcrized 
Minkowski  .solid.  (As.sumc  (t,  0)  6  /'  )•  When  the  projection  of  x  falls  on  the  /^-facc 
of  the  Minkowski  solid,  the  metric  provided  by  fp  represents  the  translational 
distance  to  a  collision.  When  the  projection  falls  outside  the  face,  the  value  of  fp 
represents  the  translational  distance  to  the  plane  of  the  /j,-face.  Hence  even  though 
there  is  ivo  convenient  way  of  talking  about  distances  between  configurations  in 
SR’’  X  •Sf.)(,l),  we  can  employ  the  values  of  C-functions  .is  a  mei.ric  on  the  distances 
of  the  moving  object  from  obstacles  at  any  configuration.  This  metric  will  become 
important  in  chapter  6. 
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Ji-i.  IlccluM(iaiil  Coiistr.aiiita 

In  chapter  2,  wc  gave  an  informal  definition  of  a  redundant  constraint  (see 
figure  there).  We  now  give  the  formal  definition  of  a  redundant  constraint  for 
a  configuration  X  6  F.  Intuitively,  a  redundant  constraint  is  one  subsumed  by 
ru'arer,  intervening  C-furictions  (lower  C-mauifolds).  Let  C  denote  the  set  of  all 
applicable,  positive- valued  C-functions  at  AT  =  (x,0).  For  each  /,  G  C,  let  s,-  be 
the  projection  into  3?'*  of  the  kernel  of  /,  restricted  to  orientation  0.  That  is, 

s.  =  {y€3?"|/.(y,e)  =  0}. 

Note  that  s,  is  the  projection  into  3?'^  of  the  tangent  hyperplane  at  0  to  the  level 
C-manifold  for  /,.  Intuitively,  s,-  is  the  plane  of  the  face  of  the  Minkowski  solid 
determined  by  /,,  at  orientation  0. 

Now,  let  h,  be  the  half-space  of  3?^  bounded  by  s,  containing  z.  Constructing 

i 

yields  a  solid  S  in  3?'^.  Those  half-spaces  bounding  5  correspond  to  the  non-redundant 
constraints  at  X. 


3.5.  Applicability  Constraints  for  type  (a)  and  (b)  C-functions 

We  are  now  in  a  position  to  derive  the  domains  of  the  C-fnnetions.  To  define  the 
applicability  constraints,  we  consider  a  family  of  C-functions  in  isolation  (that  is, 
an  environment  comprising  only  the  obstacle  D  and  the  moving  polyliedron  A).  We 
perform  an  analysis  to  see  what  generators  can  interact  at  what  orientations.  While 
C-functions  are  defined  on  the  “negated  object”  0/1(0),  applicability  constraints 
are  defined  from  the  “positive  object”  A(0). 

Definition:  Consider  a  constraint  c,  generated  by  (ga.fffc)  where  the  pair  {gti,gb)  >8 
either  (a)  a  face  of  A  and  a  vertex  of  D,  (b)  a  vertex  of  A  and  a  face  of  D,  or  (c)  an 
edge  of  A  and  an  edge  of  B.  Wo  say  c  is  applicable  at  orientation  0  if  some  pure 
translation  of  A(0)  can  bring  g,i(0)  in  contact  with  gi,,  such  that 

i{/i(e))n*(D)  =  0. 
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Soo  figure  3.1.  Let  /((-))  be  a  face  on  a  ttioviiig  polyhedron  /1(0),  with  a  normal 
/V((~)).  Let  hj  he  a  vertex  on  obstacle  li.  {f,bj)  generates  a  type  (a)  constraint.  Let 
R  be  the  set  of  adjacent  vertices  of  bj  on  the  edge  graph  of  H,  that  is, 

/?  =  {  b„  (E  verL{B)  |  6b„f)Sbj  7^  0  }. 

It  is  instructive  to  parse  the  definition  for  R.  (Recall  that  6  denotes  coboundary).  6b, i 
is  the  set  of  edges  incident  at  b,,.  If  two  vertices  b,,  and  bj  have  disjoint  coboundaries, 
then  they  are  not  adjacent  on  the  edge  graph  of  U.  If  their  coboundaries  overlap, 
then  the  common  element  is  the  edge  connecting  6„  and  bj. 

Tlicorcni  IJI.l:  A  type  (a)  constraint  generated  by  (/,  bj)  is  applicable  at  orientation 
0  if,  and  only  if,  for  all  bn  E  R, 

bn  ■  N{e)  -  bj  ■  N{e)  >  Q.  (3.1) 

If  the  type  (a)  constraint  is  applicable,  then  (3.1)  holds  for  a// vertices  6„  of  B. 
We  will  show  that  considering  the  vertices  in  R  provides  a  necessary  and  sufficient 
condition  for  applicability. 

I’roof:  (<^)  Observe  that  applicability  is  invariant  under  translation.  We 
transform  the  workspace  so  that  the  plane  of  /(0)  contains  the  origin.  Then  for 
I  G  X  •  iV(0)  is  the  perpendicular  distance  of  x  from  the  plane  of  the  constraint. 
Since  face  normals  arc  outward-directed,  when  this  distance  is  positive,  then  x  lies 
above  the  plane  of  /(0).  If  (3.1)  is  true,  then  when  bj  is  brought  to  rest  on  the 
plane  of  /(0),  then  bj  ■  N{G)  =  0.  Now,  for  all  E  R,  b,,  ■  W(0)  >  0.  Thus  all 
adjacent  vertices  to  bj  arc  on  or  above  the  halfspacc  boundary.  Since  A  and  B  are 
convex,  their  interiors  cannot  intersect. 

(=4)  If  we  can  bring  bj  in  contact  with  /(©)  while  maintaining  the  disjoint 
interior  criterion,  then  we  have  bj  ■  W(0)  =  0.  No  6„  E  R  can  dip  below  the  surface 
of  /(0),  since  then  the  interiors  of  A  and  B  would  intersect,  lienee  each  6„  must 
lie  sonu!  distance  d  >  0  above  the  plane  of  /(0).  I 


Now,  lei  /  be  a  face  of  B  with  normal  N .  (Sec  figure  3.2).  Let  a,  be  a  vertex 
of  A,  and 

^  =  {  a„  €  verL{A)  \  6a„  Q  <5a,  7^  0  } 
be  the  vertices  adjacent  to  a,  on  the  edge  graph  of  A. 

Theorem  Fir. 2:  A  type  (b)  constraint  generated  by  (a,,  /)  is  applicable  at  orientation 
0  if,  and  only  if,  for  all  a„  €  R, 

ar.(0)  •  N  -  0,(0)  N  >0.  (3.2) 

I’roof:  Symmetric  case  of  Theorem  (III.l).  | 

Consider 

9k{e)  =  .  N(0)  -  bj  ■  N(0)  (3.3) 
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as  a  mapping  g*  :  50(3)  — ►  9?.  Wc  call  a  type  (a)  applicability  constraint  function 
(ACF).  (There  are  several  ACFs  for  one  type  (a)  C-function — or  indeed  for  any 
C-function,  and  they  are  indexed  here  by  k).  For  the  symmetric  case  from  (3.2),  we 
call 


gk{e)  =  «n(e)  •  N  -  a.(e)  -  n  (3.4) 

a  type  (b)  ACF.  The  region  on  50(3)  where  g*  is  positive-valued  defines  a  half¬ 
hyperspace  of  S0(3)  (see  figure  3.3).  (3.2)  and  (3.1)  define  the  applicability  region 
for  a  type  (a)  or  (b)  constraint  as  the  intersection  of  these  half-hyperspaccs.  This 
yields  the  conjunction  promised  earlier: 


^  =  {e€50(3)iA(9t(e)>o)}. 


A  C-function  c  is  applicable  if  and  only  if  for  a  configuration  (i,0),  each  of  c’s 
ACFs  is  positive  (or  zero)  at  0,  that  is,  0  6  Ac-  The  number  of  ACFs  for  a  type 
(a)  or  (b)  constraint  is  equal  to  the  cardinality  of  'lie  coboundary  of  the  generating 
vertex  (which  is  the  same  as  |W|). 

3.6.  Applicability  Constraints  for  Type  (c)  C-fiinctions 

Determining  the  applicability  regions  for  type  (c)  C-functions  (generated  by 
edge  edge  interactions)  turns  out  to  be  a  bit  more  grueling.  Wc  can  derive  a  set 
of  ACFs  for  type  (c)  constraints  which  arc  analogous  to  gi^  in  (3.3)  and  (3.4).  The 
conjunction  of  these  type  (c)  ACFs  is  a  necessary  but  not  sufficient  criterion  for 
applicability.  The  positive  conjunction  (the  intersection  of  half-spaces  where  the 
type  (c)  ACFs  are  positive)  forms  two,  disconnected  regions  in  50(3).  It  will  become 
apparent  shortly  how  these  regions  arise,  but  let  us  pause,  before  bringing  in  some 
complicated  machinery,  to  survey  their  topology.  In  one  region  A  the  type  (c) 
constraint  is  applicable,  in  the  other  A\  it  is  not.  To  determine  which  region  0  is 
in,  wc  use  a  set  of  related  functions  termed  disambiguating  applicability  constraints 
(DACs).  Fortunately,  the  symmetric  region  A'  where  the  ACFs  are  positive  but 
the  constraint  is  not  applicable  is  disconnected  from  the  valid  applicability  region 
A  (where  the  ACFs  arc  positive  and  the  constraint  is  applicable)  by  a  region  A, 
where  the  ACFs  do  not  hold  (see  figure  3.4).  We  will  demonstrate  that  since  A  is 
disconnected  from  A',  it  is  possible  to  plan  continuous  paths  within  A  with  heed 
only  for  the  basic  type  (c)  ACFs.  Doth  type  (c)  ACFs  and  DACs  are  functions  of 
the  form  g  :  SO{li)  — ►  3?  ;  however,  they  arc  considerably  more  complicated  than 
(3.3)  and  (3.4),  above. 
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Figure  S  I.  The  space  50(3)  showing  A,  A',  and  K.  The  type  (c)  ACFs  determine  whether  O  is 
ill  the  set  A  or  in  'I'lic  DACs  determine,  for  0  €  A\JA',  wlielhcr  0  is  in  ^  or  A'. 

3.6.1.  The  Dasic  ACFs  for  Type  (c)  Constraints 

Let  c  be  a  type  (c)  constraint  generated  by  the  pair  of  edges  (coiCi)-  As 
usual  Ca(0)  denotes  Ca  rotated  to  orientation  0.  We  will  define  type  (c)  ACFs 
which  provide  a  necessary  criterion  for  applicability.  In  conjunction  with  the  DACs 
(below),  the  type  (c)  ACFs  form  a  complete  characterization  of  the  applicability  of 
type  (c)  C-functions.  We  employ  the  following  construction:  imagine  trying  to  make 
the  midpoints  of  Ca  and  ej,  touch  while  maintaining  the  disjoint  interior  criterion 
for  A  and  D.  We  then  allow  A  to  pivot  about 


V  —  mid(eft)  =  mid{ea(©))  (3.5) 

while  maintaining  disjoint  interiors.  Keeping  (3.5),  for  what  orientations  (values  of 
0)  are  the  interiors  of  A  and  B  disjoint? 


1.36 


i 

Figure  3.5.  A  section  view  through  Cj,  (ej  is  orthogonal  to  the  pnge). 

Let  US  denote  the  vertices  of  ta  and  ej  as  follows:  (see  figures  3.5  and  3.6) 
vert{ea)  —  (a,,  a,+i)  and  vert{ei,)  =  (6y,6y+i).  Now,  Co  bounds  2  faces  fi  and  /2  on 
*  A]  let  their  normals  be  N\  and  N-z-  Similarly,  let  the  normals  for  the  faces  /a  and 

/l  cobounding  be  Na  and 

Theorem  III.3:  If  a  type  (c)  constraint  generated  by  (eo,et)  is  applicable  at 
orientation  0,  then 


-dx{Q)di{Q)  >  0  (3.6) 

and 

-da(e)<i,(e)  >  0  (3.7) 

where 

d,(e)  =  bj  ■  Ni(0)  -  inid(et) .  N,(0)  (3.6a) 
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Figure  3.6.  A  scrlion  view  through  f.(e)  's  orthogonal  to  the  page). 

d2{0)  =  bj  •  N2{0)  -  mid(et)  •  N'iie) 

c?;j(0)  =  a,(0)  ■  N3  -  mitl(ea(0))  •  N3 

(f4(0)  r=  a,(0)  ■  -  niid(ca(0))  • 


(3.66) 

(3.7a) 

(3.76) 


(We  express  (3.6-7)  in  this  form  rather  than  as  di(0)d;^(0)  <  0  in  order  to  preserve 
the  positive  sign  convention  for  all  the  ACFs). 

Proof:  Refer  to  figures  3.5  and  3.6.  Again,  since  applicability  is  invariant  under 
translation,  we  transform  the  workspace  so  that  nnid(et)  is  at  the  origin.  With 
mid(e„(0))  fixed  at  mid(e(,),  d,(0)  for  t’  =  1,2  is  the  distance  of  bj  above  the  plane 
of  /v;  for  t  =  3,4,  this  is  the  distance  of  a,(0)  above  the  plane  of  /,.  We  allow 
Co  to  rotate  about  v  =  mid(c4)  with  3  degrees  of  freedom.  Observe  that  a,  and 
ai^\  may  not  dip  below  the  surface  of  D,  and  that  bj  and  bj^.]  may  not  fall  below 
the  surface  of  A.  This  is  clearly  enforced  by  considering  only  the  planes  of  the 


faces  cobounding  €„  and  tf,.  If  the  type  (c)  constraint  is  applicable  at  0,  then  di(0) 
and  d-j(O)  can  never  both  be  positive,  nor  both  negative,  for  in  these  cases  cj  will 
intersect  the  interior  of  A.  We  see  this  as  follows:  If  d|{0)  <  0  and  di{&)  <  0,  then 
bj  is  inside  both  halfspaccs,  and  some  point  on  the  line  segment  (mid(e/,),  6y)  must 
be  inside  A.  If  di(0)  >  0  and  >  0|  then  is  inside  both  half  spaces,  and 

some  point  on  the  line  segment  (mid(et),  6j^i)  must  be  inside  A. 

Hence  d|(0)d2(0)  <  0.  This  immediately  yields  (3.6).  A  similar  and  symmetric 
argument  yields  (3.7).  | 


3.7.  Disambiguating  Applicability  Constraints  (DACS)  for  Type  (c) 
Constraints 


I 

The  basic  type  (c)  ACFs  take  into  account  edge- edge  interactions,  but  do  not 
model  the  interactions  of  the  faces  they  bound.  In  order  to  preserve  the  disjoint 
interior  criterion,  wc  introduce  Disambiguating  Applicability  Constraints  (DACs)  as 
I  follows.  DACs  arc  constraints  on  the  tangent  vectors  to  faces  cobounding  ej  and  Ca! 

assuming  that  the  basic  ACFs  have  determined  that  G  E  A\J  A' ,  DACs  discriminate 
between  A  and  A'.  In  fact,  the  DACs  are  necessary  and  sufTicicnt  conditions  for 
r  applicability.  Wc  split  the  type  (c)  applicability  computations  between  the  basic 

I 

type  (c)  ACFs  and  the  DACs  for  reasons  relating  to  the  algebra  system,  which 
is  described  in  chapter  4.  Our  proofs  draws  heavily  on  constructions  employing  a 
separating  plane. 

The  Separating  Plane  Construction 

•loin  the  midpoints  of  6^(0)  and  ej  together  as  usual.  Consider  the  plane  P 
containing  v  =  mid(e„(0))  =  mid(c/,),  whose  normal  is  ea(0)  X  cj.  Assume  without 

■ 

I  loss  of  generality  that  e<,(0)  X  ej,  7^  0.  P  contains  both  e^,  and  ea(©).  Suppose  that 

the  type  (c)  ACFs  for  constraint  c  arc  positive-valued  (or  zero),  i.e.,  (3.G)  and  (3.7) 
hold.  Hence  each  vertex  of  ej  is  on  or  above  the  plane  of  one  face  cobounding 
I  Ca(0)>  Jind  each  vertex  of  e„(0)  is  on  or  above  the  plane  of  one  face  cobounding  ej. 

Refer  to  figures  3.6  and  3.5  once  more.  Dy  reason  of  the  ACF  values  for  c  and  the 
convexity  of  A  and  D,  some  open  halfspace  Pn  of  51?'^  which  is  bounded  by  P  must 
contain  i(/i)  entirely,  and  some  open  halfspacc  P4  bounded  by  P  must  contain 
I  t(A(0))  entirely: 


PC  «;(P/j) 

»  A(0)  C  k(P4). 

(Recall  that  k,(S)  denotes  the  closure  of  a  set  S'.  k{S)  —  i{S)\J  dS). 

I  Now,  if  c  is  not  applicable,  then  i(/l(0))  fl  ^(P)  7^  0-  This  means  that  A  C  k(P„) 

also,  since  unless  P,i  —  /^/i,  then  P  would  separate  f(/l(0))  from  i(P).  Wc  conclude 


T 


that  for  all  0'  €  t(i4(0'))  C  Ph  and  i(/i)  C  Pn-  Hy  a  symmetric  argument,  for 

all  0  G  >?,  plane  P  separates  i[A[Q))  from  i{P).  To  summarize:  If  the  constraint 
c  generated  by  (e„,e(,)  is  applicable  at  orientation  0,  then  i(/l(0))  f)  =  0- 
Therefore  there  exists  a  separating  plane  between  i(yl{0))  and  i{H).  On  the  other 
hand,  if  r  is  not  a|)plicable,  there  exists  no  such  separating  plane,  for  then  the 
interiors  could  not  intersect.  Furthermore,  if  e„{0)  X  Cj,  0,  then  there  exists 
exactly  one  separating  plane  that  contains  all  four  points  uer<(c„(0))  (J  t;er^(e^,).  We 
formalize  these  results  in  the  following  lemma: 

In  this  lemma,  we  abbreviate  Ca(0)  by  e„,  and  /A(0)  by  A. 

Lemma  IJf.d.l:  (Existence  and  uniqueness  of  the  separating  plane).  Join  togctlier 
the  midpoints  of  e„  and  e^.  Assume  that  e„  X  cj,  ^  0.  'fhe  constraint  c  generated 
by  (en,Ci)  is  applicable  if,  and  only  if,  the  plane  P  containing  Cu  and  separates 
the  interior  of  A  from  the  interior  of  D. 

Ppoof:  (<=)  If  P  separates  i{A)  from.  i{P),  then  z(.d)ni(/^)  “  Therefore  the 
constraint  c  is  applicable.  | 

Proof:  (=»)  If  c  is  applicable,  then  there  exists  exactly  one  separating  plane  between 
i(A)  and  i{B),  and  this  plane  is  P.  To  sec  this,  first  observe  that  if  i(A)  Cl  i(B)  ~  0» 
then  by  convexity  there  must  exist  some  separating  plane.  Assume  that  this  plane 
does  not  have  normal  e,,  X  c/,.  In  this  case,  the  plane  cannot  contain  both  and 
e/,.  Since  the  plane  contains  the  midpoints  of  both  edges,  it  must  intersect  either 
e„  or  Ch  in  a  non-parallel  cut.  But  in  this  case,  the  plane  intersects  the  interior  of 
either  A  or  B.  Thus  it  cannot  be  a  separating  plane.  Since  there  must  exist  some 
separating  plane,  it  must  have  normal  e„  X  cj.  | 

The  strategy  for  defining  DACs  is  as  follows.  For  each  face  cobounding  ej,,  we 
choose  a  point  in  the  interior  of  that  face.  The  basic  type  (c)  ACh’s  ensure  that 
Cf)  is  outside  the  interior  of  A(0),  and  that  t’a{0)  is  outside  the  interior  of  B]  the 
DACs  ensure  that  the  faces  cobounding  lie  on  the  opposite  side  of  P  from  the 
the  faces  cobonnding  e„(0).  If  the  type  (c)  constraint  is  applicable,  then  i(A(0)) 
must  lie  in  a  half-space  bounded  by  P  complementary  to  the  half-space  bounded 
by  P  containing  i[B).  The  DACs  ensure  that  if  the  faces  cobounding  e„(0)  lie  in 
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Figure  3.7.  Tho  Tangent  Vectors  T3  and  T4  to  the  faces  cohonnding  e^. 

«:(Pyi),  then  the  faces  cobounding  ej,  must  lie  in  with  P^  7^  P/j.  Since  and 

B  are  convex,  this  suffices  to  show  that  A  and  B  lie  in  complementary  half-spaces 
bounded  by  P. 

The  vertices  of  ea(0)  and  ej,  lie  on  P.  Let  pi,  p2  be  points  in  the  interior  of  the 
faces  cobounding  e^,  and  pa,  p^  be  points  in  the  interior  of  the  faces  cobounding 
ej,.  The  DACs  ensure  that  p\  and  p2  lie  on  one  side  of  P,  and  that  pa  and  p^  fie  on 
the  other. 

The  points  inside  the  faces  cobounding  ct  and  Cq  are  chosen  as  follows.  For 
each  edge  e  on  B  and  A,  wc  construct  a  pair  of  tangent  vectors,  {T\,T2),  where 
Ti  and  T2  are  tangent  and  interior  to  the  faces  cobounding  e.  Tj  and  T2  are  also 
perpendicular  to  e.  For  an  edge  Ca  on  A,  {7’i(0),  Tj(0))  will  clearly  rotate  with  e„ 
and  A,  maintaining  these  criteria.  The  tangent  pair  for  cj,  is  shown  in  figure  3.7. 
P’ormally,  we  proceed  as  follows: 


Ocfliiition:  A  tangent  vector  to  3?^  (O’Neill  (1966))  is  a  pair  G  3?  X  3?  i 

interpreted  as  the  vector  v  applied  to  point  p.  We  will  sometimes  write  Vp  for  (v,p), 
or,  when  there  is  no  ambiguity  about  the  point  of  application,  we  simply  write  v. 

Definition:  Consider  an  edge  e  on  a  polyhedron  P.  Let  J\,  /a  be  the  faces  that 
cobound  e,  and  let  N\,  N2  be  their  normals.  A  tangent  pair  for  e  is  a  pair  of  tangent 
vectors  to  3?’’,  {Ti,T2),  both  applied  to  mid(e).  T,  is  perpendicular  to  e  and  to  TV,, 
and  it  is  directed  into  the  interior  of  /,  when  applied  to  mid(c)  (t  =  1,2).  In  other 
words, 

Ti  =  k{NiXe)  (t  =  1.2) 

where  k  £  {  +1,  -1  }  is  chosen  to  orient  T,  into  the  interior  of  TV,  X  e  indicates 
the  cross  product  of  TV,  and  the  directed  edge  vector  for  c. 


Refer  to  figures  3.7  aiul  3.8.  We  will  now  construct  DACs.  Let  be  the 

tangent  pair  for  Cf,,  and  let  N:(,  N/[  be  the  normals  to  the  faces  cobounding  ej,.  Let 
(T'i(0),  7^(0))  be  the  tangent  pair  for  £,,(0),  and  let  A^i(0),  A^‘j(0)  be  the  normals 
to  the  faces  coboundi.ig  e„(0).  Thus  7’,  ■  =  0  (for  i  —  1,2,3,'!).  Keeping  with 

this  numbering  convention,  let  /,  be  the  face  with  normal  N^.  As  usual,  we  imagine 
joining  together  the  midpoints  of  ej  and  e„(0). 

Let  7V/>(0)  be  the  normal  to  the  plane  P,  that  is,  A^/<(0)  —  eu(0)  X  Cfc-  Assume 
without  loss  of  generality  that  7//>(0)  7^  0.  The  plane  containing  mid(e„(0))  = 
mid(c/,)  with  normal  7//<(0)  also  contains  ea(0)  and  ej.  We  construct  DACs  which 
ensure  that  i(A(0))  is  on  one  side  of  P,  and  that  t(7?)  is  on  the  other  side.  To 
ensure  that  the  [joints  inid(ej,)  +  73  and  niid(eft)  +  7’i  lie  on  the  same  side  of  P,  we 
have  the  constraint 

sign(73  •  Ni>{Q))  =  sign(Tt  •  iVy.(0)) 

v.hich  may  be  written 

in  ■  Nr{e)){T,  •  'V/.(e))  >  0. 

Assume  without  loss  of  generality  that  the  signs  are  non- zero.  The  case  where  one 
sign  is  zero  is  easily  handled  by  examining  the  other  sign.  To  ensure  that  the  points 
niid(eu(0))  -f  7i(0)  and  mid(e„{0))  -t-  72(0)  He  on  the  same  side  of  P,  we  have  the 
symmetric  constraint 

sign(7’,(0)  •  7V/.(0))  =  sign(T2(0)  ■  /V/.(0)) 

Now,  we  must  ensure  that  the  two  half-spaces  are  complementary.  This  is  enforced 
by  insisting  that  the  signs  are  opposite.  All  of  the  following  must  be  true: 

k„  =  sign(T3  •  /Vp(0)) 


sign(7’,  •  yV,.{0)) 

(3.8a) 

-.=  sign(7’,(0)./V,.(0)) 

=  sign(72(0)  •  Af/.(0)) 

(3.86) 

k/[  kii 

(3.8c) 

Equations  (3.Sa-c)  embody  the  DACs  we  require. 


Theorem  111.4:  Let  c  be  a  type  (c)  C-function  generated  by  Assume  the 

tangent  pairs  for  ei,  and  and  normals  to  the  faces  cobounding  ej  and  e,j  are  as 
above.  Then  c  is  applicable  if,  and  only  if,  the  all  the  DACs  (.3.8a-c)  hold. 

Proof:  (=^)  Assume  the  type  (c)  constraint  is  applicable,  but  that  at  least  one  of 
(3.8a-c)  is  false.  VVe  will  demonstrate  a  contradiction.  Join  the  midpoints  of  e„(0) 
and  Cfc,  as  usual.  If  any  of  the  DACs  is  false,  then  P  does  not  separate  i(A(0))  from 
a  contradiction.  | 

Proof:  (<^)  We  show  that  if  tlie  DACs  hold,  then  c  is  applicable:  if  tliesc  conditions 
are  true,  then  P  is  a  separating  plane.  Therefore  the  interiors  cannot  intersect,  and 
c  is  applicable.  | 
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;}.8.  On  the  Structure  of  the  Type  (c)  Applicability  Regions  on  50(3) 

In  this  section,  we  prove  ;i  theorem  on  the  structure  of  the  regions  A,  A',  and 
A  for  type  (c)  constraints,  (see  figure  3.-1)  which  yields  an  iinincdialc  coin[)leteness 
result  for  our  formulation  of  ACI's  and  DACs.  As  promised,  we  will  show  that  A 
and  A'  are  disconnected  on  5’0(3),  and  that  the  region  A  separates  them.  Our 
proof  draws  heavily  on  constructions  employing  a  se[)arating  plane  (lemma  III.'l.l). 

d'lieorcni  111.5:  A  disconnects  A  from  A'  on  50(3). 

Proof:  We  first  observe  that  by  definition, 

>^U(>^U>^')  =  ^‘50(3) 

(see  (3.6),  (3.7)  for  confirmation).  Recall  the  separating  plane  construction:  we  saw 
that  for  all  0'  G  A',  z(/\(0'))  C  P/r  and  i{B)  C  P//.  bet  P/;  denote  the  interior 
of  tlie  complement  of  P ^  —  i(3?'*  ■  P//).  By  a  symmetric  .irguiiicnt,  for  all 

0  C  A,  plane  P  separates  ?(/l(0))  from  i{B).  If  )■'>  path-connected,  then  there 

exists  a  continuous  function,  p  ;  *  50(3),  sucli  that  p(0)  =  0,  p(l)  =  0^  and 

p(f')  C  A\JA'.  I'urthermore,  if  is  path-connected,  then  for  all  t  6  f’,  ^-ithcr 

C  Pn,  or  i(.l(/)(i )))  C  Pit  (assume  without  loss  of  generality  that  for  all 
',  t'(,  X  e„(p(t))  ;/  D).  .\ote  tliat  for  all  t, 

Hence  in  traversing  tlie  p.ith  /<  in  rotation  space,  A  is  required  to  “flip”  over  P 
from  f’li  to  /•*/;,  without  its  interior  ever  intersecting  /’.  This  is  clearly  impossible 
if  continuity  is  to  be  preserved.  | 

3.9.  Oi  ieiitirig  l’yp<'  (c)  ( loiistr.aints 

Consider  iillixinj',  tiiid(e„(0))  to  v  -  mid(c/,)  ;is  usual.  Refer  once  more  to  3.5 
and  3.6.  'The  cross  product 


when  applied  to  v  will  for  some  0  point  out  of  P/j  and  into  P^;  for  other  0, 
7V/-(0)  will  point  into  P/j  and  out  of  Pj\.  (Assume  for  now  that  N/*(0)  0.)  Hence 

for  some  orientations  N/'(0)  is  the  correct  (unnornialized)  real-space  normal  for 
constraint  fo*"  other  orientations  we  must  employ  — A^/>(0).  When  applied 

to  V,  the  real-space  normal  kNi>[Q)  (for  k  G  {+1,-1  })  must  always  point  out  of 
P/i  and  into  P^.  The  following  rule  for  choosing  k  is  stated  without  proof: 


k  =  sign(Af/*(0)  •  /(0)) 


(3.12) 


where  /(©)  =  Ti(0)  -f  72(0). 


However,  it  is  easy  to  see  that  we  need  not  compute  this  dot  product  each 
time  we  use  the  C-function.  k  (and  the  orientation  of  N]>{Q))  will  be  invariant  in 
regions  of  A  where  the  signs  of  the  ACFs  are  invariant.  For  example,  if  k  is  positive 


for  some  Q  ^  A  and 


d;j(0)  >  0  and  ^^(0)  <  0, 


(3.13) 


then  clearly  wherever  (3.13)  holds,  then  k  must  be  positive.  Also,  wherever 

^3(0)  <  0  and  d.t(0)  >  0, 

k  must  be  negative.  This  argument  should  be  quite  obvious  if  the  reader  imagines 
how  the  cross  product  of  the  edges  changes  as  pivots  about  mid(e(,).  This  leads  to 
the  following  simple  algorithm  for  orienting  a  type  (c)  C-function  c.  Es.scntially,  we 
can  just  compute  (3.12)  once,  and  record  the  signs  of  the  ACFs  at  that  orientation. 

(i)  For  some  0,  compute  the  values  of  di{Q)  [i  =  1,2,  3,  4)  for  the  type  (c)  ACFs. 

If  c  is  not  applicable,  then  stop. 

(ii)  If  k  has  not  been  computed  yet,  calculate  k  as  in  (3.12).  (Assume  k  -/-  0).  Record 
the  signs  of  (i;)(0)  and  fi.|(0)  for  c.  We  call  this  pair  of  signs  the  sign  map  for  c. 

(iii)  If  a  A:  and  sign  map  have  been  computed  for  c,  then  compare  the  recorded  sign 

map  to  the  current  sign  map  for  ^3(0)  and  If  the  sign  maps  are  equal, 

use  k  to  orient  c;  otherwise  use  —k. 


3.10.  Singularities  and  Special  Cases 


Our  analysis  of  type  (c)  ACFs  and  DACs  assumes  that  e„(0)  and  ej,  are  never 
aligned,  i.e.,  that  their  cross-product  is  never  zero.  In  addition,  our  algorithm 
for  orienting  type  (c)  C-functions  assumes  that  no  function  d,  is  zero.  The  cross 
product  will  be  zero  when  ea(0)  is  parallel  to  e^,  and  an  ACF  will  be  zero  when 
either  £,,(0)  is  aligned  with  a  face  cobounding  ey,  or  when  e;,  is  aligned  with  a  face 
cobounding  e„(0).  In  practice,  these  special  cases  will  arise  frequently.  Fortunately, 
they  can  be  ignored.  Consider  the  following:  The  vertices  of  e,,  generate  type  (b) 
constraints  with  the  faces  cobounding  ej;  and  the  vertices  of  generate  type  (a) 
constraints  with  the  faces  cobounding  e„.  In  the  cases  where  6,^(0)  is  aligned  with 
Ci  or  a  face  cobounding  (or  in  the  symmetric  case),  some  of  these  constraints 
will  also  be  applicable.  In  these  aligned  cases  we  say  that  the  type  (c)  constraint 
is  subsumed  by  the  neighboring  type  (a)  and  (b)  constraints,  because  the  disjoint 
interior  criterion  will  be  enforced  by  the  type  (a)  and  (b)  constraints  alone.  This  can 
bo  seen  as  follows:  (see  Figure  3  5)  suppose  some  ACF,  for  example  dn,  is  zero- valued 
at  0,  and  that  Q  £  A.  Then  both  mid(ea(©))  ^md  o,(0)  can  be  brought  to  rest 
on  the  plane  of  while  preserving  the  disjoint  interior  criterion.  Since  a,q.i(0) 
is  also  lies  on  e„(0),  it  too  may  be  brought  to  rest  on  the  plane  of  /s.  Clearly, 
the  type  (b)  constraints  generated  by  (a,-,/3)  and  must  also  be  applicable 

at  orientation  0.  At  this  aligned  orientation,  the  type  (c)  constraint  ensures  the 
following:  while  mid(e<,(0))  is  on  the.  plane  of  f,\,  (i,(0))  must  also  lie  on  the  plane 
of  fi-  This  is  precisely  the  condition  enforced  by  the  equivalent  pair  of  type  (b) 
constraints.  Symmetric  arguments  hold  for  the  other  ACFs. 

3.11.  Level  ACFs 

P'or  ACFs,  there  is  an  analogous  concept  to  a  level  C-Manifold.  Let  g  :  S'0(3)  — » 
9?  be  an  ACF  for  a  C-function  c.  An  ACF  lioundary  is  the  space  of  rotations  where 
c.  is  applicable  and  g  is  zero: 

ker  g  =  {  0  €  .^0(3)  |  g[Q)  =  0  }. 

A  Level  ACF  is  the  space  of  rotations  where  c  is  applicable  and  g  is  some  constant 
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value  1: 


{0GSO(3)U(0)  =  O- 

Recall  the  geometric  interpretation  for  ACFs.  Consider  a  type  (b)  constraint  (see 
Ggure  3.5).  A  path  p  :  50(3)  along  a  level  ACF  for  the  constraint  (a,,  /u) 

would,  if  the  midpoints  of  the  edges  were  affixed,  preserve  a,(0)  at  a  constant 
height  above  the  plane  of  /s. 

3.12.  A  Note  on  the  Computation  and  Algebra  of  Applicability  Constraints 

The  implemented  planning  system  contains  an  algebra  system  (described 
in  chapter  4),  which  performs  the  computations  relevant  to  the  applicability 
constraints.  We  would  like  to  make  the  computation  as  simple  as  possible,  for 
otherwise  an  implementation  might  be  infeasible. 

Wc  have  .ehown  that  there  are  four  types  of  computations  foi  the  applicability 
constraints: 

(i)  Typo  (a)  ACFs  (3.3)  which  determine  the  applicability  of  type  (a)  C-functions. 

(ii)  'I'ypc  (b)  ACFs  (3.4)  which  determine  the  applicability  of  type  (b)  C-functions. 

(iii)  [Basic]  Type  (c)  ACFs  (3.6)  and  (3.7),  which  provide  a  necessary  but  not 
sufficient  condition  for  the  applicability  of  type  (c)  C-functions. 

(iv)  DACs  (3.8a-c)  which  provide  necessary  and  sufficient  conditions  for  type  (c) 
applicability. 

However,  it  is  not  hard  to  show  that  the  real-valued  functions  for  (iii)  and 
(iv)  arc  composed  of  simple  type  (a)  and  (b)  ACFs.  Wc  will  demonstrate  this  as 
follows.  Let  Sl'^  be  the  space  of  normals  to  planes  in  3?'^.  Note  that  is  of  course 
isomorphic  to  3?^  We  now  define  the  functions  F,\  and  F/f  to  model  the  computation 
of  ty|)e  (a)  and  (b)  ACFs.  These  functions  will  be  composed  to  compute  the  more 
complex  type  (c)  ACFs  and  DACs.  Let  /'^  and  Fit  be  real-valued  functions 

Fa,  F„  :  X  3?'’  X  9? 


where 


and 

Fn{a,„  a.-,  N,  0)  =.  a„(©)  •  N  -  a.(0)  •  N. 

Clearly,  and  Fn  can  be  used  to  corr.i  ute  ACFs  for  all  type  (a)  and  (b) 
constraints.  They  can  also  be  used  to  compute  type  (c)  ACFs  as  follows: 

j  f ®)>  ^  2; 

cij(  C)J  —  ^ 

[Fiilai,  mid(e„),  Af,-,  0),  if  i  =  d,  4. 

N/‘[0)  is  already  computed  as  the  real-space  normal  for  a  type  (c)  C-function. 
With  Ni‘[0)  in  hand,  DACs  can  be  computed  using  Fn  and  F^\.  This  is  because 
DACs  arc  essentially  constraints  on  tangent  vectors  to  the  faces  of  the  polyhedra 
in  question,  and  the  tangent  space  of  51?^  is  isomorphic  to  its  normal  space.  We 
will  show  how  to  compute  DACs  using  type  (a)  and  (b)  ACFs.  Our  trick  for 
rotating  a  tangent  vector  (u,p)  simply  involves  rotating  the  line  segment  [p,v  +  p) 
to  (p(0),  [u  +  p](0))-  I'"or  example, 

7’i(0)  •  Np{e)  =  F/,(mid(c40o))  +  7’,(0„),  mid(e„(0o)),  Nr{e),Q^ 

Here  0o  denotes  some  fixed  orientation.  Typically  ©o  is  the  identity  element  for 
the  rotation  group,  i.e.,  it  denotes  no  rotation  at  all,  and  will  be  the  orientation 
in  which  the  polyhedra  arc  given,  and  in  which  the  tangent  pairs  arc  initially 
computed.  In  particular,  [7',(0())](0)  —  7’j(0). 

Our  retluction  of  all  applicability  computation  to  a  few  simple  functions  is 
partially  motivated  by  aesthetics,  and  partially  by  the  design  of  an  algebra  system 
for  our  planner.  The  reduction  will  admit  a  simpler  and  more  elegant  design. 

3.12.1.  A  Conjecture 

Let  us  make  one  final  comment  on  type  (c)  ACl's.  For  each  type  (c)  C-function, 
there  are  two  type  (c)  ACFs.  One  type  (c)  ACF  (3.6)  is  the  product  of  two 
type  (a)  ACFs;  and  the  other  (3.7)  is  the  product  of  two  type  (b)  ACFs.  These 
products  are  constrained  to  be  negative.  In  practice,  we  would  probably  wish  only 
to  compute  the  value  of  each  subresiilt  (<i,)  for  each  type  (a)  and  (b)  ACF,  and  then 
compute  a  logical  conjunction  to  determine  when  one  is  negative  and  the  other 


l.'iO 


positive,  instead  of  computing  their  product.^  We  conjecture  that  the  composition 
of  type  (c)  C-functions  and  ACFs  reflects  the  underlying  algebraic  structure  of  these 
constraints:  observe  that  each  type  (c)  face  of  the  Minkowski  solid  B  Q  A{Q) 
is  the  composition  (by  direct  sum)  of  an  edge  on  A  and  an  edge  on  B:^ 

A.6  =  e.(0)0e6.  (3.H) 

Similarly,  the  (real-space)  normal  Na^b  to  such  a  face  is  the  composition  (by  vector 
cross-product)  of  an  edge  on  A  and  and  edge  on  B: 

Na,b  =  eaie)  X  et.  (3.15) 

In  this  chapter  we  have  derived  a  new  symmetry,  a  symmetry  for  the  ACFs  of  type 
(c)  constraints.  In  particular,  it  is  now  clear  that  type  (c)  ACFs  are  the  composition 
(by  scalar  multiplication)  of  a  pair  of  type  (a)  or  (b)  ACFs. 


^Tliis  iipproncli  i.s  lakc'ii  for  (.tie  irTipIrincnted  planner. 
^I'apjalion;.  (3. I'll  and  (3.15)  are  from  l,ozano-I’ere7  (1983). 


Mathematical  Tools  for  Motion  Planning 
in  a  Six  Dimensional  Configuration  Space 

4.1.  Introduction 

Ou  r  earlier  presentation  of  representational  issues  and  applical)ility  constraints 
in  X  50(3)  addressed  basic  theoretical  issues  for  the  motion  planning  problem. 
In  this  chapter  we  discuss  s|)eciDc  issues  which  were  critical  for  the  implementation 
of  the  planning  system  described  in  chapter  2.  The  fundamental  issue  is  the 
intersection  problem  in  high-dimensional  configuration  spaces: 

(i)  How  do  we  intersect  high-dimension.al  level  C -Manifolds  to  construct  an 
intersection  manifold? 

(ii)  How  do  we  intersect  a  trajectory  in  configuration  space  with  C-Spacc  constraints? 
ICxamples  and  applications  of  these  results  may  be  found  in  chapters  1  and  2. 

VVe  will  proceed  as  follows.  First,  as  a  “simple”  exam{)lo,  we  will  solve  these 
f)rol)lems  for  the  configuration  space  9i~  X  5'.  For  this  space  the  algebra  is  not 
unreasonable  and  illustrates  some  of  the  complexities  of  planning  for  the  dDOF 
c.ise.  However,  in  5)?'*  X  SO{'.\),  the  equations  for  some  constraints  (notal>ly,  type  (c) 
constraints)  can  fill  several  pages.  For  this  reason,  I  first  computed  the  general  form 
of  the  intersections  for  an  arbitrary  constraint,  and  then  solved  all  intersections 
using  Maesyma  (LCS  (J983)).  The  results  were  then  optimized  and  compiled  into 
Lisp.  I'or  all  pr.ictical  pnrpo::es  these  results  .arc  in  machine  readable  form  only.  For 


example,  using  Euler  Angles  parameterized  by  0  =  [rp,0,<f))  for  three-dimensional 
rotations,'  a  type  (b)  constraint  in  Macsyma  becomes: 

(  (-XC  (AI)  ‘XC  (NG J)  «C0S  (PHI)  -XC  (AI)  •  YC  (NG J)  *SIN (PHI)  )  *C0S  (THETA) 

+XC  (AI)  »ZC  (NG  J)  *  SIN  (THETA)  -YC  (AI)  ♦  YC  (NG  J)  *COS  (PHI) 

♦YC(AI)*XC(NGJ)*SIN(PHI)) 

*COS(PSI) 

+  ( (YC (AI)  ‘XC (NGJ) ‘COS (PHI) +YC (AI) ‘YC (NGJ) »SIN(PHI) ) *C0S (THETA) 

-YC(AI)  *ZC(NGJ)  *SIN  (THETA)  -XC(AI)  »YC(NGJ)  »CQS(PHI) 

+XC(AI)*XC(NGJ) *SIN(PHI)) 

•  SIN  (PSD  -ZC  (AI)  *ZC  (NGJ)  *CQS  (THETA) 

+  (-ZC  (AI)  •XC (NGJ)  *CQS  (PHI)  -ZC  (AI)  ‘YC  (NGJ)  •SIN (PHI) )  •SIN (THETA) 

-YC(NGJ)  *COS(PHI)+XC(NGJ)  *SIN  (PHI)  •ZC  (NGJ)  •Z+YC  (NG J)  •Y+XC  (NGJ)  •X 
-ZC  (BJ)  •ZC  (NGJ)  -YC  (BJ)  •YC  (NGJ)  -XC  (BJ)  •XC  (NGJ)  . 

This  is  the  simplest  of  the  constraints;  a  type  (c)  constraint  is  over  10  times  as  long. 
For  3?'^  X  50(3)  our  approach  has  been  to  (1)  derive  these  constraints  (and  the 
ACFs)  from  some  arbitrary  representation  for  rotations,  (2)  reduce  each  constraint 
to  a  series  of  simpler,  canonical  forms  which  arc  linear,  bilinear,  or  quadratic  in  the 
terms  of  interest,  and  (3)  develop  simple  mathematical  procedures  for  operating  on 
the  canonical  forms. 

For  example,  to  construct  an  intersection  manifold  for  n  constraints,  we 
essentially  need  to  solve  a  set  of  n  simultaneous  equations,  each  of  the  form 


/(AT)  =  0. 


(X  e  X  so(3)) 


We  proceed  as  follows.  I.et  D  —  {  i,  y,  z,  t/»,  0,  (^  }  be  the  set  of  all  the  degrees 
of  freedom.  First  we  select  P,  a  subset  of  6  —  n  clcnicnts  of  D.  P  will  parameterize 
the  intersection  manifold.  The  variables  in  P  will  be  the  free  variables  which  the 
planner  can  choose;  the  variables  D  —  P  will  vary  dopendently  with  P  so  as  to  stay 
on  the  intersection  manifold.  Mechanically,  this  entails  (l)  solving  the  n  constraints 
simultaneously  eliminating  all  but  one  variable  in  D  —  P,  and  (2)  expressing  all 
dependent  degrees  of  freedom  D  —  P  in  terms  of  the  free  variables  P. 

The  canonical  forms  arc  expressions  for  C-functions  which  make  explicit  the 
cocdicicnts  of  the  dependent  variables  {P  —  P)  tlu-msclves,  and  of  the  sines  and 
cosines  of  these  variables.  13  complicated  equations  describe  the  canonical  forms 
'IOiiUt  mo-  iiii plcnicnlcd  as  roUil.'uiii  inalrircs  in  tlir  ptimicr.  Sci'  Syinoii  (1971). 


of  a  C-function,  and  9  equations  are  needed  for  a  type  (a)  or  (b)  Complete 

Macsyma  listings  of  these  procedures  are  provided  in  an  appendix.  Defore  wading 
into  these  waters,  however,  let  us  turn  our  attention  to  the  conGguration  space 
3?'“^  X  S'. 

We  will  adhere  to  the  definitions  and  conventions  established  in  chapter  3. 
4.2.  The  Intersection  Problem  in  3?^  X  S' 

The  find-space  and  find-path  problems  in  3?^  X  S'  are  of  considerable  intrinsic 
interest.  We  have  suggested  that  good  algorithms  for  the  two  dimensional  Movers’ 
problem  could  be  developed  by  planning  along  the  intersections  of  constraints.  Some 
of  the  necessary  theoretical  tools  for  this  approach  arc  presented  in  this  section. 
These  results  illustrate  the  principles  necessary  for  planning  along  intersection 
manifolds  in  9?'^  X  SO(3).  The  derivations  are  simpler  because  (1)  the  constraints 
are  simpler  and  (2)  the  applicability  regions  arc  merely  sectors  on  the  unit  circle.  A 
complete,  general  path  planner  has  been  implemented  for  this  problem  (see  Brooks 
and  Lozano-Perez  (1983)).  This  section  serves  both  as  a  pedagogic  example  and  as 
a  presentation  of  a  new  approach  to  the  planning  problem  in  9?^  X  6’'. 

To  plan  paths  along  the  intersections  of  constraints,  we  must  be  able  to  construct 
the  ititerscction  manifold  of  some  set  of  constraints.  To  preserve  tradition  (see 
Brooks  and  Lozano-Perez  (1983),  for  example),  we  will  call  any  level-0  C-manifold  a 
C-Kurface.  A  C-surface  is  the  space  of  configurations  where  a  C-function  is  applicable 
and  zero- valued.  C-surfaces  are  interesting  because  they  bound  C-Space  obstacles. 
We  will  derive  the  form  of  the  intersection  of  atiy  two  C-surfaces  in  3?^  X  5'.  Each 
C-surfacc  is  a  2-dimcnsional  manifold  in  9?“  X  5',  and  their  intersection  manifold  is 
a  curve  p  in  9?“  X  .  We  derive  a  curve  p  which  is  parametric  in  0.'*  Since  there  are 
2  types  of  C-surfaces  (type  (a)  and  (b)),  there  are  3  types  of  intersection  manifolds. 

'1.2.1.  The  IntcrsoctuJii  of 'I'wo  C-Surfaces  in  9?^  X  -b’' 

We  describe  a  technique  for  finding  the  intersection  of  two  C-surfaces  for  the 

two  dimensional  mover’s  problem  with  rotations.  Throughout  this  discussion  of 

^VVe  .sec  now  why  it.  was  tlcsiralilc  to  cx()r«'ss  all  At’Ks  aiul  l)A('s  as  roinposilions  of  type  (a) 
an.l  (h)  A(M’a. 

''|{(■lall  that  (T,y,0)  is  a  l.ypical  point,  ifi  the  ('-Sparc  SH'*  X  S* . 


9?“^  X  s’,  we  will  employ  the  abbreviations  C  =  cosO  and  S  =  sinO.  The  surfaces 
are  embedded  in  a  4-dimcnsional  manifold  and  expressed  as  functions  on  (.t,  y,  C,  S) 
with  the  added  constraint  that  =  1.  A  system  of  equations  for  two  surfaces 

can  then  be  solved  for  x  and  y  in  terms  of  C  and  S. 

Two  type  (a)  constraint  surfaces  are  functions  of  the  form  f{x,y,0)  —  0,  for 
example: 

sin{0  +  \i)y  +  cos(fl  +  X,)i  —  ||l»y||  cos{0  +  X,-  —  qy)  —  ||a,])  cos(X,-  —  ?;,■)  (al) 

sin(0  +  X,')y  +  cos{0  +  X,')x  -  ||6y||  cos((?  +  X,-  —  qj)  —  ||a,-||  cos(X'  —  r)-)  (a2) 

Similarly,  two  type  (b)  surfaces  are: 

su\{<t>j)y  +  cos{<j)j)x  -  ||a,!|  cos{0  -  4>j  +  7?,)  -  116yl|  cos{<i>j  ~  qy)  (61) 

sin(^6y)y  +  cos{<;6y)i  -  ||a/|)  cos(0  -  (f)'j  +  y,')  -  j|6y||  cos{<p'j  ~  7y)  (62) 

Refer  to  figure  (4.1).  Here  the  a,’s  are  vertices  of  the  “negated”  moving  polygon 
(O-A  in  Lozano- Perez  [1981,  1983]),  in  its  local  coordinate  system,  rji  is  the  angle 
the  line  from  the  origin  of  that  coordinate  system  to  the  point  a,  makes  with  the 
coordinate  system’s  x  axis,  and  X,  is  the  angle  made  by  the  normal  to  the  segment 
from  a,  to  Similarly  the  6y’s  are  the  vertices  of  a  convex  obstacle  polygon,  7y 

the  orientation  of  the  line  from  the  origin  to  6y,  and  ^y  the  orientation  of  the  normal 
to  the  segment  from  6y  to  6y,  p  The  parameter  0,  a  parameter  of  the  configuration 
space,  measures  the  angle  between  the  x  axes  of  the  object  and  obstacle  coordinate 
systems. 

Type  A  constraints  can  be  thought  of  as  being  generated  by  a  face  (edge)  of 
the  moving  object  A  coming  into  contact  with  a  vertex  of  an  obstacle  13,  and  a 
type  li  constraint  as  a  vertex  of  A  coming  into  contact  with  a  face  (edge)  of  H. 


««rtM 


Figure  8.  The  two  typei  of  lurfacei  can  be  deSned  by  bring  tbe  reference  point  of  the 
negative  of  moving  object  A  into  contact  with  a  vertex  and  an  edge  of  fixed  obilacle  B. 
Both  are  defined  over  a  range  of  orientationa  f. 


l''i(;\irr  4.1.  An  lllii.slrjillori  of  tlic  toriiirt  in  »-qii:itioii»  (al)  ;iiul  (1>1).  I{('[)riiil<'(l  with  portnission 
from  Mrooks  ;ui<l  Lorano  I’t  rc'r  (IflSil). 


liach  constraint  is  valid  only  over  a  fixed  range  of  0.  ror  type  A  surfaces  the  range 
is  given  by  0  G  [ipj  i  —  —  and  for  type  If  surfaces  by  0  G  {<i>j  —  Xi,  <f>j  —  Xt  i 

By  applying  trigonometric  reductions  we  can  express  these  constraints  as 
follows  (only  (al)  and  (bl)  are  shown): 

cos(Xj)5t/  +  C  sin(X,)y  —  sin(X,).9i  +  C  cos(X,)i 
+  sin(X,-  -  7j)|IM1-5  -  Ha, II  cos(X,  -  r/,) 

-  C'cos(X,  - '7j)||6j||  (al) 


s\n{<j)j)y  +  cos(<^jjx  -  ||a,||  sin(<^y  -  7j,)S 

-  C||a,||  cos((jl.y  -  rji)  -  ||6ji!  cos((^j  -  y)  (61) 

Where 

C  =  cos  0,  S  —  sin  9. 

Now,  we  can  consider  a  pair  of  these  equations  as  a  system  in  four  variables, 
{x,y,C,S),  and  proceed  to  solve  (al)  and  (b2),  (bl)  and  (b2),  and  (al)  and  (bl)  for 
X  and  y.  For  example,  the  intersection  of  two  type  (a)  surfaces,  (al)  and  (a2)  is  a 
curve 

P  ■  ra\f]ra2  3?^  X  5* 

where  raif]fa2  C  S'*  denotes  the  intersected  applicability  constraints  for 
(al)  and  (a2).  Although  the  solutions  arc  in  the  variables  C  and  S,  we  can  use 
C  =  COS0  —  cosr  and  S  =  sin0  —  sin  r  to  generate  the  curve  of  intersection 
in  9?^  X  5*.  Because  of  their  excessive  length,  these  equations  may  be  found  in 
a[)pcndix  I. 

4.2.2.  Intersecting  Trajectories  with  C-surfaccs 

A  General  Discussion  for  3?"*  X  5'  and  X  •SO(.'l) 

In  order  to  motivate  a  discussion  of  the  intersection  problem  for  trajectories 
and  C-surfaccs,  we  now  introduce  the  problem  in  a  context  which  will  be  expanded 
’  Soiirrc:  'I’Ik'  list,  '.tircc  are  cxfcrplcil  frimi  Unioks  and  l.ozanod’eri  /,,  IIOSH). 


upon  in  chapter  5.  The  goal  of  this  discussion  is  to  illustrate  how  the  intersection 
results  are  used  in  the  planner  described  in  chapter  2. 


In  principle  it  is  possible  to  intersect  arbitrary  trajectories  with  C-surCaces  — 
such  trajectories  could  translate  and  rotate  simultaneously.  Once  an  intersection 
is  found,  we  must  then  determine  whether  (I)  the  C-surface  is  applicable,  and  (2) 
whether  it  lies  on  the  boundary  of  a  C-Space  obstacle.  The  question  of  applicability 
may  be  resolved  a  priori  by  maintaining  and  updating  an  accurate  set  of  applicable 
constraints  as  the  planner  moves  through  rotation  space.  This  set  is  called  the 
applicabilily  set.  As  the  planner  moves  from  0  to  Q',  the  updating  algorithm 
must  detect  which  constraints  have  expired  (ceased  to  be  applicable)  and  which 
new  constraints  have  been  activated  (become  applicable).  The  expired  constraints 
arc  deleted  from  the  applicability  set,  and  the  new  constraints  are  added.  In  this 
manner  the  trajectory  will  bo  intersected  only  with  the  applicable  constraints. 
Another  approach  involves  intersecting  the  trajectory  with  all  C-surfacos,  and  then 
Gilding  tlie  first  applicable  intersection  on  the  boundary  of  a  C-Space  obstacle. 
The  first  strategy  is  more  general  in  that  it  decomposes  the  image  of  the  trajectory 
into  equivalence  classes  where  the  applicability  set  is  invariant.  Hence  it.  can  in 
principle  be  used  to  map  out  these  equivalence  classes  on  SO{3).  However,  for 
most  environments  the  latter  strategy  runs  faster,  although  both  techniques  can 
be  shown  to  have  the  same  asymptotic  complexity.  Both  algorithms  have  been 
impleinerited''’  and  tested,  and  arc  presented  later  in  chapter  5. 

There  arc  also  two  ways  to  determine  if  an  intersection  lies  on  the  boundary 
of  a  C-Space  obstacle.  Let  X  be  the  intersection  point  of  a  trajectory  with  an 
applicable  C-surface  /.  Then  X  lies  on  the  boundary  of  a  C-Space  obstacle  bounded 
by  /  if  either  of  the  following  holds: 

(i)  All  :ip(dicable  C-fiinctions  in  /’s  family  are  negative  or  zero- valued  at  X 

(ii)  If  the  projection  of  X  into  real-space  lies  within  the  displaced  face  of  the 
Minkowski  solid  corresponding  to  the  generators  for  /. 

Correctness  Argument  Let  us  briefly  discuss  why  (i)  and  (ii)  are  equivalent.  The 
correctness  of  (i)  is  obvious,  since  the  C-Space  obstacle  is  constructed  as  the  Gnite 
intersection  of  half  hy[)crspaccs,  each  of  which  is  defined  by  a  real-valued  function 
on  C-Spnee.  bet  S  denote  the  face  of  the  Minkowski  solid,  and  x  the  jirojection  of 

'for  5?'  X  .SY^(.’i)  iiol,  for  X  A''. 

’’The  fiuriilii  ol’  a  (’-fiiiicl  ion  is  dediied  in  d.2. 


159 


r 


n 


r 


u 


« 


the  iiitersocliori  i)oiiit  into  real-space  (i.c.,  X  —  (x,  0)).  We  will  demonstrate  that 
(i)  (ii). 

(^)  Suppose  (i),  but  not  (ii).  We  demonstrate  a  contradiction,  x  must  lie  on 
(he  plane  of  5,  even  though  x  ^  S,  since  that  is  how  the  C-functions  are  defined 
(X  could  not  be  an  intersection  point,  olherwise).  Recall  that  the  normals  of  the 
faces  (and  planes)  bounding  the  Minkowski  solid  are  delined  to  be  outward-directed 
from  the  interior.  Since  the  Minkowski  solid  is  convex,  the  plane  of  S  bounds  a 
half-space  entirely  containing  the  solid.  If  x  is  not  within  S ,  then  it  must  be  outside 
tJie  plane  of  some  other  face.  S' ,  which  shares  an  edge  with  S.  Hut  in  this  case, 
the  C-functiori  corresponding  to  S'  will  be  positive- valued:  a  contradiction. 

(^)  The  Minkowski  solid  is  convex.  If  x  G  S,  then  it  is  behind  (or  on)  the 
plane  of  every  other  faces  of  the  solid.  The  C-functions  arc  defined  in  terms  of  the 
distance  of  x  from  these  planes,  which  must  be  negative  (or  zero).  | 

One  further  note:  suppose  that  all  intersections  with  C-surfaccs — including 
non-applicablc  C-surfaccs  have  been  sorted  along  the  image  of  the  trajectory  in 
C-Space.  Then  if  X  is  the  first  intersection  for  which  (ii)  holds,  then  /  is  applicable 
and  X  lies  on  the  boundary  of  the  C-Space  obstacle.  Again,  both  approaches  have 
been  implemented,  and  the  results  arc  discussed  later. 

Intersecting  'IVajectorles  with  C-surfaces  in  9?^  X  5’ 

We  will  now  present  methods  for  intersecting  pure  transl.ational  and  pure 
rotational  trajectories  with  C-surfaces  in  X  5'’.  Note  that  as  long  as  every  path 
of  interest  lies  entirely  within  open  sets  of  9?^  X  S\  then  for  every  such  path  there 
exists  a  hornotopically  eepiivalent  path  composed  of  “staggered”  (Mire  translations 
and  f)ure  rotations.  We  assume  such  {)aths  can  be  expressed  as  (piecewise)  linear 
functions  of  some  parameter.  Intersecting  such  a  path  witli  a  C-surfacc  entails 
finding  the  zeroes  of  the  associated  C-function  (with  respect  to  the  (larameter). 

Purr.  Transliiliojiat  Paths.  Note  that  (al)  a:id  (bl)  are  linear  in  x  and  y.  At  a  fixed 
orientation  their  (irojcction  into  real  space  is  a  line.  A  juire  translational  path  is 
also  a  line.  Clearly  then,  iidersection  of  a  pure  translational  path  with  a  C-surfacc 
is  trivial. 
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Pure  Rotaiional  Paths.  A  pure  rolational  path  is  a  linear  function  from  to  S'. 
Intersecting  such  a  path  with  a  C-surfacc  involves  fintling  tlie  zeros  (witli  rcsjject 
to  0)  of  the  C-function  at  a  constant  translation.  Observe  that  C-surfaces  (al)  and 
(bl)  are  linear  in  C  and  S,  that  is,  they  can  be  exi)rcsscd  as 

EiC  ^^S  +  Ea  =  0  ('l.l) 

where  the  terms  E,-  (for  i  =  1,2,3)  vary  only  with  x  and  y.  The  zeros  of  (d.l)  are 
not  hard  to  find.  First  we  note  that  (d.l)  can  be  expressed  as  a  pure  quadratic  in 
C  (or  S),  and  that  solving  a  quadratic  for  its  zeros  is  easy.  (We  must,  of  course, 
check  for  the  first  applicable  zero  which  is  on  the  boundary  of  a  C-Space  obstacle), 
'riiis  method  is  not  the  best  because  of  susceptibility  to  numerical  problems  and 
singularities.  Happily,  such  equations  arise  frequently  in  robot  kinematics;  Paul 
(1981)  describes  a  stable,  singularity-free  calculation  for  the  zeros  of  exactly  this 
form  of  trigonometric  equation. 

I'ractical  Note 

The  reader  will  notice  that  motion  sliding  along  an  intersection  manifold 
in  X  5'  will  not  in  general  be  a  pure  translation  or  rotation.  We  have  not 
derived  the  results  for  intersecting  arbitrary  trajectories  with  C-surfaces  in  31"  X  S', 
although  in  principle  it  is  possible  to  do  so.  Note  that  any  such  sliding  motion 
can  be  approximated  as  closely  as  desired  by  a  sequence  of  pure  translations  and 
rot.itions,  and  furthermore,  .any  such  “approximating”  planner  will  be  comi)leto  (in 
the  sense  discussed  above)  if  the  “sliding”  planner  is  complete. 

Furthermore,  our  purpose  here  is  a  theoretical  analysis  in  low  dimensions 
whicli  still  illuminates  some  of  the  staggering  difficulties  in  3?'*  X  50(3).  As  it  turns 
out,  with  the  aiKlitional  degrees  of  freedom  in  3?'  X  50(3),  this  turns  out  to  be 
considerably  less  of  a  restriction. 

d.3.  Ilelated  Problems  in  3J^  X  5' 

There  arc;  a  number  of  interesting  related  problems  in  3?'^  X  5'.  The  first 
addresses  techniques  for  '‘sli<ling”  along  one  geometric  constraint  (C-surface). 
Sliding  is  a  usa-ful  way  to  circumnavigate  obstacles:  it  can  also  be  used  to  slide  to  an 
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intersection  rnanirolci.  The  second  result  is  of  use  in  the  find-space  and  coordinated 
motion  problems,  and  involves  characterizing  the  minimum  clearance  to  a  C-surface 
in  X  S'*.  Again,  these  results  are  presented  not  only  for  their  intrinsic  interest, 
but  also  as  an  cxi)osition  of  some  of  the  algebraic  tcchniciues  required  and  as  an 
illustration  of  the  complications  arising  in  high-dimensional  configuration  spaces. 

d.I3.l.  TechniqiioH  for  Moving  Along  C-Surfaces  in  9?^  X  5’ 

In  this  section  we  present  techniques  for  moving  along  a  C-Surfacc.  We  could 
imagine  using  these  methods  to  move  to  the  nearest  “edge”  (C-Surface  inbusection), 
for  example.  A  level  C-Surface  is  defined  via  a  function  /(x,  y^O)  =  k  for  k  constant. 
/  is  exactly  of  form  (al)  or  (61)  (above),  and  the  level  surface  in  X  S'*  is  all 
points 

L={X€9?^Xr/l/(X)  =  /c}, 
where  ry  C  S’  is  the  0  applicability  range  for  /. 

Define  a  hyper[)l.anc  in  !)?^  X  S'*  as  the  set 

P  =  {X  e  X  S’*  I  X  •  ff  =  -h^  }, 
where  II  =  {h\,  h^,  h^). 

We  intersect  the  level  surface  /y  with  the  hyperplane  P  to  obtain  an  intersection 
curve  p  :  I^  —*  X  5*.  The  ecpiation  for  this  curve  for  both  type  (a)  and  (b) 
C-surfac.es  may  be  found  in  appendix  I. 

4.3.2.  Cliaractorizing  Clear.ance  to  a  C-Surfacc 

It  would  be  very  useful  to  characterize  the  minimum  clearance  to  a  C-surfacc. 
The  result  could  be  ai)plied  in  the  coordinated  motion  probhun  to  determine  where 
two  molulc  objects  could  possibly  interact.  In  the  find-space  problem,  we  coidd  use 
clearance  information  to  maximi/,e  the  clearance  to  a  constraint  while  placing  one 
object,  in  order  to  leave  room  for  another.  We  would  like  to  answer  the  question: 

•  I'or  a  point  6^^  i':  9i",  at  ivlint  orientation  is  bjy  elosr.st  to  a  C-surface,  and 
what  is  rninirnum  directed  clearance  vector  al  that  orientation? 

Using  bagrange  multipliers,  we  c.an  minimize  a  function  f[x,y,0)  subject  to  a 
constraint  (j{x,y,0)  -  0  by  con;  1 1  m  l  ing  the  auxiliary  functioti 


II (x,  y,  0, 1)  =  /(x,  y,  0)  ~  I(j{x,  y,  0) 

and  simultaneously  solving  the  partial  derivatives  of  II.  In  our  case,  (j  will  define 
a  C-surface,  and  /  will  be  a  distance  function.  Now,  the  rotational  dimensions 
cannot  be  treated  uniformly  in  establishing  a  metric,  so  we  will  define  distance  in 
I'aiclidean  sf)acc.  Minimizing  the  square  of  the  translational  distance  suHices  for 
our  purposes.  Hence, 

/(i,  y,  0)  =  {x-  b^)'^  +  (y  -  byf. 

Differentiating  II  gives  us  a  system  of  four  equations.  Solving  these  equations  for  i, 
y,  0,  and  I  is  not  trivial.  We  provide  the  solutions  and  their  derivation  in  appendix 
I.  (Solutions  are  given  for  both  type  (a)  and  type  (b)  C-siirfaces). 

4.4.  The  Intersection  Problem  in  X  50(3) 

In  this  section  we  extend  the  previous  examples  of  intersection  problems  to 
the  fi-dimensional  C-space  v  ,S0(3)  *\.t  this  point  we  must  commit  ourselves 
to  a  particular  representation  for  rotations.  The  implemented  planner  uses  a 
rotation  matrix  specified  by  Euler  Angles.  Implementing  a  different  representation 
for  rotations  (such  as  spherical  angles,  quaternions,  or  joint  angles  for  a  Cartesian 
Manipulator)  would  merely  require  replacing  the  Maesyma  rotation  abstraction 
ROTATE-VECTOR  with  the  appropriate  new  function  (and  recompiling  the  algebra 
.system).  The  lOuler  Angles  are 

The  intersection  problems  in  9?'^  X  50(3)  arc  as  follows.  With  each  problem 
we  give  the  riiotivation  for  attacking  it. 

(i)  Intersecting  (level)  C-surf:ices.  (Necessary  to  construct  the  intersection  manifold). 

(ii)  Int.ersecting  Level  ACl's.  (Interesting  theoretical  (juestion;  relates  to  planning  on 
dilferenl  kinds  of  interst'clion  manifolds,  and  exploiting  coherence  in  C-Space 
constraints). 

(iii)  Intersecting  (’-Surfaces  with  Level  A(’!'’s.  (Same  as  (ii)). 

(iv)  Intersecting  'I'ra jectories  with  C^-.surf:u-es.  (Indicates  that  we  may  have  hit  a 
C-Spnre  obstacle). 
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(v)  Intersecting  Trajectories  with  ACFs.  (Indicates  that  a  constraint  has  expired 
(ceased  to  be  applicable)). 

Note  that  we  never  have  to  intersect  a  trajectory  with  a  DAC,  since  any  path 
straying  out  of  a  type  (c)  constraint’s  applicability  region  must  first  violate  an  ACF 
boundary  (see  Theorem  III. 5).  Since  all  ACFs  can  be  composed  out  of  type  (a)  and 
(b)  ACFs,  we  need  only  deal  with  three  distinct  kinds  of  functions  on  3i'*  X  50(3) 
and  two  on  50(3).  In  the  context  of  this  section  the  term  ACF  is  used  to  refer 
only  to  the  basic  type  (a)  and  (b)  ACFs  out  of  which  all  ACFs  and  DACs  may  be 
composed. 

Our  approach  is  as  follows;  We  express  all  C-functions  and  ACFs  in  certain 
canonical  forms.  The  Maesyma  proceilures  to  derive  these  forms  are  provided  in  an 
appendix.  We  then  develop  certain  operations  which  are  defined  on  any  function 
expressed  in  these  forms.  Throughout  this  discussion  of  9?^  X  50(3),  we  use  the 
notation  Ca  =  cos  a  and  Sn  =  sin  a  where  a  g  {■>p,0,(/)}.  Most  of  the  claims  in 
this  section  should  be  self-evident  when  the  rotation  matrix  Z{&)  for  Euler  Angles 
is  considered. 

Claim  .{-I'  All  C-functions  are  affine  in  x,  y,  and  z.  This  is  obvious,  since  -5(0)  is 
a  linear  transformation.  | 

Claim  4-S:  While  expres.sions  for  C-functions  and  ACFs  can  contain  cross- terms  of 
the  form  CnS[),  SaSf),  or  C„Cf),  it  should  be  clear  that  a  ^  that  is,  Cc,  can 
always  be  expressed  as  an  affine  function  of  5q. 

To  derive  this,  consider  the  definition  of  a  C-function  (equation  (4)  in  chapter 
3)  once  more: 

f„{x,  0)  (N{e),  x)  -  (iV(0),  (a.(0)  +  6y)) 

Only  the  term  (/V(0),  a,(0))  could  result  in  any  troublesome  terms.  For  a  type  (b) 
constraint,  N(0)  is  a  fixed  vector.  For  a  type  (a)  constraint,  A’(0)  is  a  rotated 
normal  of  a  face  of  A,  and  we  have 

(/V(0),a,(0))=.(N,a.). 
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F'inally,  for  type  (c)  constraints,  /V(0)  is  the  cross  product  of  eu(0)  and  e(,.  This 
results  only  in  cross-terms  of  different  angles: 

(a,(0),e„(0)  X  Ci)  =  (a,(0),^ai,  ,(0)  -  a,(0)j  X  c*) 

=  {«.(©),  a.  1 1(©)  X  ej,  -  a,(0)  X  Cfc) 

=  (ai(0).a.  i  i(0)  X  Cfc) 

=  (eA,a,(0)  X  a,  H(©))- 

A  proof  for  the  ACFs  is  very  similar.  | 

4.4.1.  Canonical  Forms  for  C-functions  and  ACFs 

Definition:  The  Linear  Form  for  a  C-function  /  :  X  50(3)  — >  9?  is  an  equivalent 

expres.sion 

/(i,  y,  z,Q)  —  E[X  +  E-iV  -I-  E^z  -t-  E^, 

where  Ei  :  50(3)  — >  3?  (for  i  =  1,2,  3,  4). 

Definition:  A  Trigonometric  Quadratic  Form  (TQF)  (in  (p)  for  a  C-function  /  is 
an  equivalent  expression 


f{i,  y,  z,  i/>,  0,  4>)  =  Fi  sin  <i>  -h  F-z  cos  <j)  -f  F3, 


where 

/';:9?'^X(^,0)-^9?.  (1-3=  1,2,3) 

Definition:  A  Trigonometric  Quadratic  Form  (TQF)  [in  (^)  for  an  ACF  g  :  50(3)  — > 
3?  is  an  cciuivalent  expression 

j7(V’,  0,  (j))  --  Cl  sin  (l>  +  Czcosd)  +  O3, 


where 

(t- 1,2,3) 

The  TCJl's  are  defined  here  in  <j>  -of  course  we  must  also  define  the  TQFs  in  ip 
and  in  0  in  the  natural  way.  <f)  will  be  our  typical  example  angle  in  this  discussion, 
however. 


Before  we  proceed  let  us  provide  some  intuition  for  these  delinitions.  Imagine 
deriving  a  linear  form  for  a  C-function,  and  setting  the  expression  equal  to  zero. 
'I'he  result  is  just  an  expression  whose  coefTicicnts  make  explicit  how  the  plane 
e(juation  of  the  face  of  the  Minkowski  solid  changes  with  rotation. 

A  TQF  (in  (j>)  is  just  a  way  of  expressing  C-functions  and  ACFs  in  terms  of 
the  coeflicicnts  of  sin  and  cos  Linear  forms  and  TCJFs  will  be  useful  canonical 
forms  for  the  intersection  problem  in  5R'*  X  50(3).  It  is  imi)ortapt  to  realize  (hat  the 
cix  incients  A’,,  F,,  and  G,  are  actually  functions  on  the  other  degf'  ^s  of  freedom. 

We  see  immediately  from  claims  (4.1)  and  (4.2)  that: 

CMaiiii  4.3:  FWery  C-function  can  be  expressed  as  a  linear  form  and  as  a  TQF  in 
ij},  0,  and  <^;  similarly,  every  ACF  can  be  expressed  as  a  TQF  in  t/»,  0,  and  <f>. 

4.4.2.  Intersecting  C-surfaccs  in  9?'*  X  50(3) 

When  intersecting  C-siirfae''s  in  9?^  X  5',  we  essentially  eliminated  variables  in 
a  system  of  o<iuations.  This  corrcspotids  exactly  to  “spending”  degrees  of  freedom 
to  comply  to  two  constraints.  In  9?“*  X  5',  there  were  few  choices  for  which 
variables  to  eliminate.  However,  in  9?'^  X  50(3),  we  have  many  more  degrees  of 
freedom,  and  hence  there  arc  more  choices  for  how  to  solve  the  intersection  of 
a  set  of  constraints.  For  example,  to  construct  the  intersection  manifold  of  three 
constraints,  we  could  spend  all  the  transl.itional  degrees  of  freedom,  which  would 
result  in  {)arameterizing  the  intersection  manifold  by  (r/i,  <1,  0).  Alternatively,  we 
could  in  [)ririciple  elimiri.ite  the  rotational  degrees  of  freedom  and  parameterize 
the  intersection  manifold  by  [x,y,z).  In  the  former  case,  we  leave  [xl),0,tf))  as 
indc'pendent  degrees  of  freedom:  parameterizing  the  intersection  manifold  simply 
involves  solving  (he  3  constraints  simultaneously  for  x,  y,  and  z  in  terms  of  (i/:,  0,  0). 
'Fo  move  along  their  intersection,  we  are  free  to  plan  :iny  values  for  {xp,0,4>),  and 
the  parameters  for  the  translational  degrees  of  freedom  will  vary  so  as  to  comply 
to  the  simultaneous  set  of  constraints.  Obviously  the  choice  of  which  degrees  of 
freedom  should  parameterize  an  intersection  manifold  is  important;  linear  forms 
and  'FQFs  give  us  a  general  way  of  attacking  it.  This  approach  is  best  illustrated 
through  the  following  cxamidcs: 
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Example  (i).  A  C-surface  in  linear  form  is  an  expression  for  a  C-function  in  linear 
form  set  equal  to  zero.  Two  C-surfaces  expressed  in  linear  form  may  be  intersected 
to  yield  a  '1-dimensional  intersection  manifold  parameterized^  by  [z,  ip,  0,4)).  This 
amounts  to  simultaneously  solving  the  equations 

f{x,y,  z,  0)  =  Ell  -f  E-iV  +  E3Z  -f-  Ei  =  0 

g{x,  y,  z,  0)  =  E[x  -t-  E^y  -h  Ejz  -f  E^  =  0 

by  first  eliminating  x  and  then  solving  for  y.  This  yields  expressions  for  x  and 
y  in  terms  of  [z ,  ip ,  0 ,  4>)',  we  say  that  [z,ip,d,4>)  form  a  '1-parameter  family  for 
the  intersection  manifold,  and  that  x  and  y  comply  to  the  C-surfaces  /  and  g  as 
{z,ip,0,4>)  are  varied. 

This  intersection  has  the  following  geometric  interpretation.  Imagine  holding 
orientation  constant  at  0i.  Then  Ej  and  E[  are  all  constant  also.  Intersecting  / 
and  g  at  a  constant  orientation  is  equivalent  to  intersecting  two  planes  in  Hi'.  The 
intersection  is  a  line,  and  the  position  along  the  line  may  be  parameterized  by 
the  one  remaining  translational  degree  of  freedom,  z.  The  planes  intersected  are 
exactly  the  planes  of  the  faces  of  the  Minkowski  solid  for  /  and  g  at  orientation 
01. 

Example  (ii).  The  intersection  manifold  f{X)  —  g{X)  =  0  from  example  (i)  may 
be  intersected  with  another  C-surface,  h{X)  —  0,  expressed  in  linear  form.  Suppose 
z  is  eliminated.  Then  the  intersection  manifold  for  /,  g,  and  h  is  parameterized  by 
[ip,  0,  4>)-  The  translational  degrees  of  freedom  i,  y,  and  z,  will  be  expressed  in  terms 
of  the  rotational  degrees  of  freedom,  and  will  sli<lc  along  the  intersection  manifold 
as  rotations  arc  chosen.  The  new  intersection  manifold  f[X)  g{X)  =  h[X)  —  0 
is  a  3-dimcnsional  sub-rnanifold  of  5}'*  X  SO(3).  This  intersection  has  the  following 
geometric  significance.  Imagine  holding  orientation  fixed  at  0)  once  more.  The 
intersection  at  a  fixed  orientation  of  /,  g,  and  h  is  the  intersection  of  three  planes 
in  9?'*.  This  intersection  (if  it  exists)  is  a  typically  a  point.  If  0i  is  allowed  to  vary, 
the  intersection  point  moves.  The  coordinates  of  the  intersection  point  arc  the  x, 
y,  and  z  degrees  of  freedom  as  they  comply  to  the  intersection  manifold. 

^As.muiih'  l-li:il.  llu'  ronslraints  arc  not  (larallel,  and  that  this  is  p«)»sil)lp,  etc. 
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Examples  (i)  and  (ii)  show  how  to  spend  translational  degrees  of  freedom  to 
intersect  C-surfaccs.  In  (i),  we  saw  that  it  is  possible  to  plan  motion  along  the 
the  '1-dimensional  intersection  manifold  with  one  translational  and  three  rotational 
degrees  of  freedom,  (i)  can  be  used  to  plan  a  pure  translational  path  complying 
to  two  C-surfaces.  The  free  translational  parameter  may  essentially  be  chosen  to 
maximize  progress  in  a  search  algorithm.  This  is  precisely  how  one  “local  expert” 
in  the  implemented  planner  works. 

One  last  note  on  linear  forms:  the  discussion  and  examples  above  can  be  easily 
generalized  to  arbitrary  level  C-surfaces  (instead  of  C-surfaces  with  level  0)  by 
increasing  or  decreasing  (the  “constant”  term  in  the  linear  form)  by  a  constant 
equal  to  the  level. 

Intersecting  Two  TQEs 

Consider  a  TQF  g  (in  4>)  for  either  a  C-function  or  an  ACF,*^  and  suppose 
further  that  the  TQF  has  been  .set  equal  to  zero  so  that  it  is  actually  a  TQF  surface, 
ker  g,  by  which  we  mean  a  TQF  for  a  C-surface  or  ACF  boundary: 

P\  sin  <t)  -I-  /'2  cos  ^  -P  F3  =  0 
Such  a  TQF  can  be  expressed  as: 


[F]  +  /'tj)  cos^  4>  +  2F2/';,  cos  <f>  +  rl-  F\  =  0.  (4.2) 

'rhe  new  expression  is  <]uadratic  in  cos<f>.  (This  explains  the  name  TQF).  The 

procedure  for  intersecting  two  quadratics  is  well  known.®  Such  a  procedure  can 

be  used  to  intersect  two  qiiailratics  of  form  (4.2)  (i.e.,  with  cos(^  treated  as  the 

([uadratic  variable).  Thus  we  can  obviously  intersect  any  two  TC^F  surfaces.  This 

means  that  the  procedure  for  intersecting  two  quadratics  can  be  applied  to  TQFs  of 

(%.snrfaces  and  of  ACF  boundaries.  This  immediately  yields  an  effective  procedure 

’^Deiii'iiilirij;  dm  wlietlier  the  is  ;i  C  riirii  tioii  or  ACK,  I  he  niii(  ti()ii.s  l'\  will  have  diirereiit 

done. 'ills,  hut  l.lii  i  will  not  iiiaHi  r  for  our  ilisriission. 

”l’i)r  example,  see  Winston  and  Morn  (1?)S1),  (p.  175). 
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for  constructing  the  intersection  manifold  of  two  C-surfaccs,  two  ACF  boundaries, 
or  a  C-surface  and  an  ACF  boundary  while  spending  only  rotational  degrees  of 
freedom. 

4.4.3.  Intersecting  Trajectories  with  C-Surfaccs  and  ACF  Boundaries  in 
3?=^  X  50(3) 

In  this  section  we  extend  the  method  of  ('1.2.2)  for  intersecting  arbitrary  linear 
pure  translational  and  pure  rotational  trajectories  with  C-surfaccs  in  3?*  X  50(3). 

Pure  Translational  Paths.  A  pure  translational  path  can  not  stray  out  of  an 
applicability  region.  It  is  not  hard  to  intersect  a  linear  pure  translational  path  with 
a  C-surface.  Such  a  path  can  be  represented  as  a  line  in  3?^.  At  the  fixed  orientation 
of  the  path,  any  C-surface  can  be  represented  as  a  plane  in  3?'*.  Hence  the  problem 
of  intersecting  a  C-surface  with  a  (linear)  pure  translational  path  is  simply  the 
problem  of  intersecting  a  line  with  a  plane.  The  linear  form  of  any  C-surface 
directly  provides  the  cooflicients  of  this  plane  for  any  (applicable)  orientation. 

Note  that  in  intersecting  a  pure  translational  trajectory  from  some  configuration 
X  E  F  with  a  set  of  applicable  C-surfaces,  we  need  only  consider  C-functions  which 
are  positive- valued  at  X. 

Pure  Rotational  Paths.  We  restrict  our  attention  to  linear,  pure  rotations  in 
one  rotational  direction  (i.e.,  in  ±^,  ±0,  ±^),  for  example, 

<p{t)  =  /cQ  -f  kit  {t  e  /’) 

(for  sonic  constants  Icq  and  ki).  To  intersect  such  a  path  with  a  C-surface  (or  ACF 
boundary),  we  simply  find  the  zeros  of  the  appropriate  TQF.  For  this  example,  we 
would  use  the  TQF  (in  4>)  for  the  C-surface: 

Fi  sin  0  f-  F'i  cos  <f>  F^  =  0.  ('I -3) 

With  motion  strictly  in  <^,  the  functions  F^  will  be  constant,  and  may  be  regarded 
simply  as  the  coefficients  of  a  quadratic  form.  ('1.3)  is  easily  solved  for  the  v.alues  of 
^  which  are  its  roots  (see  section  '1.2.2).  Now,  depending  on  the  solution  technique. 


(4.3)  may  yield  several  roots.  The  correct  root  may  he  chosen  as  follows:  for  a 
C-surface,  we  choose  the  first  root  where  the  C  surface  is  ap|)licahle  for  an  ACF 
boundary,  we  choose  the  first  root  where  the  assoc  iated  (’  siirfaic  is  applu  able. 
This  last  step  requires  examining  the  other  .ACl's  for  th<  ('  surface 

Coinpicteness  and  Complexity  for  |{olational  'IVajec I  ories 

We  have  seen  that  a  continuous  {>ath  through  rot  at  ion  'p.n  i  i  an  la  ippr  o\ini  ,ited 
as  closely  as  desired  by  a  series  of  linear  mot  ions  along  tin  r  ii  ai  n  m  al  a  >  i  s  \\c  now 
show  that  the  number  of  path  segments  reepnred  grows  lim  irU  i  i!i.  r'  sidntion 
of  the  approximation  becomes  finer. 

Definition:  Let  F  be  a  vector  space,  and  /’  and  I"  be  ii  ijia  lorn  s  m  I  We  say 
that  P'  approximates  P  at  resolution  r  if  for  all  p'  c  /".  i  In-  pc  rpendn  niar  distance 
of  p'  to  P  is  less  than 

Proposition  4-4-  A  linear  trajectory  in  a  vector  space  c  an  fie  approximated  by  a 
number  of  path  segments  along  the  axes,  which  increa.ses  linearly  as  the-  rc'solution 
becomes  finer. 

Proof-.  Suppose  V  —  and  P  is  a  linear  trajectory  from  u  to  n.  Imagine 
approximating  P  by  linear  motions  along  the  i,  y,  and  5  axes.  Segment  into  k 
subpaths.  From  u,  attain  each  of  the  A:  —  1  subgoals  (and  v)  by  3  linear  motions 
(along  X,  y,  and  z)  from  the  previous  subgoal.  This  yields  a  secjuence  P'  of  3A: 
motions  which  approximates  P  at  resolution  r*.  We  can  bound  ^  from  above  as 
follows: 


~  \Vy  -  Uy\,  \v,  - 


n 

To  achieve  a  particular  resolution  r,  it  is  easy  to  choo.se  the  smallest  k  satisfying 
the  reverse  inequality.  We  see  immediately  that  k  varies  linearly  with  r.  | 

Let  the  angle  space  be  the  domain  of  a  chart  for  50(3),  as  described  in 
chapter  2.  Then  tht:  angle  space  trajectory 

p(f)  =  ©1  +  ft) 


for 


V  =  +  bO  + 

specifies  a  well-defined  trajectory  Jl{p{t))  in  50(3). 

Proposition  4-5:  We  can  approximate  p  as  closely  as  desired  by  a  sequence  {7,  }  of 
motions  in  Q'  along  the  tj),  0,  and  (j>  directions.  Furthermore,  the  size  of  the  set 
{  9t  }  grows  only  linearly  as  the  resolution  r  becomes  finer. 

Proof:  Immediate,  from  proposition  (4.4).  | 

4.5.  The  Algebra  System 

The  treatment  here  of  the  implemented  algebra  system  is  mercifully  brief. 
Given  the  discussion,  the  details,  at  least  in  principle,  should  be  easily  imagined 
by  most  readers.  In  computer  algebra  these  problems  are  well  understood,  and  the 
system  does  not  make  a  significant  contribution  to  that  field.  I  would  like  to  note, 
however,  that  the  algebra  system  is  both  massive  and  at  the  heart  of  the  planning 
system.  It  takes  12  hours  for  a  dedicated  VAX  to  optimize  and  compile  the  vector 
form  of  the  constraints  (in  Maesyma)  into  the  primitive  functions  of  the  Lisp  algebra 
system.  On  top  of  these  primitives  is  built  a  more  abstract  system,  which  (for 
example)  can  evaluate  constraints,  intersect  constraints,  intersect  trajectories  with 
constraints,  and  find  zeros  of  constraints.  The  intersection  and  evaluation  system 
has  automatic  singularity  handling  (for  division  by  zero,  imaginary  roots,  .alignment, 
etc).  For  example,  to  intersect  two  C  surfaces  (n  /dcxamj>le  (i)),  the  planning  system 
will  typically  specify  a  list  of  preferences  for  the  translation.al  [).arameterizalion  of 
the  intersection  manifold.  The  system  then  attempts  to  construct  an  intersection 
manifold  with  a  high-ranked  parameterization,  and  on  encountering  singularities 
will  back  up  and  try  again. 

It  should  now  be  clear  how  the  algebra  system  for  the  planner  is  di'signed. 
For  each  kind  of  constraint  (C-function  or  A(’l''),  the  algebra  system  contains 
procedures  which  compute  the  coellicients  of  the  linear  form  (for  C-surfact's  only), 
and  coellicients  of  tin;  'I’CJFs.  Fach  of  these  procedures  can  be  thought  of  as  a 
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function  of  (1)  the  constraint,  and  (2)  the  parameters  not  explicit  in  the  form  (for 
example,  the  rotation  parameters  for  the  x  coefTicient  of  a  linear  form).  On  top 
of  this  is  built  a  level  of  abstraction,  so  that  for  example  the  operation  “compute 
the  sin  cj>  coefficient  of  the  TQF  (in  4>)"  is  defined  on  all  constraints.  Coefficients 
of  all  possible  forms  are  described  by  a  total  of  12  coefficients  for  the  linear  forms 
of  C-functions,  27  coefficients  for  the  TQFs  of  the  C-functions,  and  18  coefficients 
for  the  TQFs  of  the  ACFs.  (These  functions  correspond  exactly  to  the  functions 
/?,,  Fi,  and  C,,  above).  All  of  these  functions  arc  constructed  and  optimized  by 
Maesyma  running  under  NIL  (Burke  (1983)),  and  then  converted  into  Lisp. 

VVe  have  also  experimented  with  precompiling  functions  for  all  possible 
intersection  manifolds  (up  to  some  degree).'*^  For  intersection  manifolds  of  degree 
2  or  3,  this  is  not  hard,  and  in  fact  w^e  have  already  illustrated  all  the  necessary 
mathematics  in  this  chapter.  Intersection  manifolds  of  higher  degrees  may  be 
constructed  by  solving  for  the  submanifold  representing  the  simultaneous  satisfaction 
of  s‘-v«'r  >1  constraints,  for  example,  three  constraints  in  linear  form  together  w'ith 
I  .VO  'real's  such  as  (‘1.2).  When  higher  degrees  are  considered,  this  becomes 
(juitr  complicated,  especially  when  we  allow  different  parameterizations  of  the 
inti  TMCt  ion  manifolds.  Construction  of  intersection  manifolds  of  higher  degree  may 
be  c.i'-icr  when  different  representations  for  rotations — such  as  unit  quaternions — arc 
employed.  'I'liis  appears  a  fruitful  direction  for  future  research.  In  practice,  we  view 
It  preferable,  whentver  possible,  to  obtain  the  values  of  coefficients  of  a  form 
.U  a  lert.iin  confii’,uralion,  an.d  then  to  plan  locally  while  keeping  these  coelficients 
fixed  'f'fius  for  example,  we  might  compute  t'.e  coefficients  of  the  linear  forms  of 
two  ('  -urface^.  at  a  given  orient.ition,  and  then  intersect  the  resulting  planes  to 
olitam  a  t rarl^lat ional  path  ;dong  their  intersection.  The  structure  of  the  forms 
make.s  this  e.isy  to  do.  For  example,  rotating  the  moving  object  (say,  in  i^)  until 
it  hits  a  (onstraint  is  mathematically  a  complicated  ojreration.  All  we  need  do, 
however,  is  find  th('  ( (u'lTicients  of  the  TQF  in  (j),  and  supply  them  to  a  procedure 
in  the  aig.ebr.i  system  which  finds  the  zeros  of  d'QF  surf.aces.  (But  see  chapter  5  for 
the  details  of  the  airplicability  set  computation). 

'“'fill'  i/rf  I  of  an  inn  inaiiiroid  i.s  siiiipty  tlie  niiniber  of  (•oMstraint.s  intersectial  there. 
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4.6.  Related  Issues  in  5R'*  X  50(3) 


4.6.1.  Normals  to  C-surPaces 

Let  /  be  an  applicable  C-function  and  X  a  configuration  on  a  level  C-surface 
for  /.  When  an  appropriate  inner  product  is  defined  on  the  tangent  space,”  the 
normal  to  the  C-surfacc  at  X  is  the  gradient  of  the  C-function  /  evaluated  at  /. 
Normals  to  C-surfaces  are  of  great  importance  for  motion  planning.  The  gradient 
may  be  computed  as  follows:  first  the  coefficients  for  the  linear  form  of  /  (evaluated 
at  X)  are  obtained; 

/(X)  =  EiX  +  E-iV  +  E32  +  E^. 

Clearly,  5^  =  ^l,  =  Ei,  and  =  E3.  To  obtain  the  partial  derivatives  in  the 

rotational  direction,  we  find  then  cocflicicnts  of  the  TQl'^s  (evaluated  at  X) 

/(X)  =  Fi  sin  4>  +  F-i  cos  -f  F3 

to  obtain 

3  f 

—  =  Fi  cos  4>  —  F2  sin  <j>. 
d4> 

4.6.2.  C-functions,  Potential  Fields,  Penalty  Functions,  and  Morse  Theory: 
A  Conjecture 

A  popular  approximate  algorithm  for  collision  avoidance  places  “potential 

fields”  around  the  obstacles  (cither  in  real  space  or  in  some  C-Spnee),  and  attempts 

to  navigate  the  reference  point  through  a  trough  of  least  resistance.  The  obstacles 

may  be  thought  of  as  having  a  “charge”  which  repels  the  robot,  and  the  goal  has  an 

inverse  charge  which  “attracts”  it.  The  potential  field  method  is  closely  related  to 

the  .so-called  “Morse  Theoretic”'^  approach  to  motion  j)lanning,  and  lends  itself  to 

fast  control-loop  algorithms  which  can  exercise  real-time  dynamic  control  of  robot 

arms  with  few  degrees  of  freedom,  in  simple  environments.  As  miglit  be  expected, 

the  method  works  best  for  robots  that  can  be  approximated  by  points  or  spheres.  A 

proper  potential  function  increases  as  the  robot  approaches  the  obstacle,  and  goes 

' '  S<'<r  .scr.  2.'t.2  and  ICrdmann  (1!)81). 

l.ak<'.s  il.M  iiniiif  from  .Mf)r;i('  Theory  in  dilli-renlial  topology. 


to  iiilinity  :it  the  obstacle  boiiiulary.  'rraclitionally,  the  potential  function  is  chosen 
somewhat  arbitrarily,  with  much  emphasis  on  the  closeness  of  the  “fit”  of  the 
potential  surfaces  about  the  real-space  obstacles,  and  with  understandable  concern 
for  the  computability  of  such  functions  by  specific  control  hardware.  With  the 
theoretical  tools  we  have  develojjed,  it  is  now  {)ossibl<;  to  give  a  /jolential  function  in 
configuration  space  which  is  “exact.”  For  a  config.u ration  X ,  let  /  be  a  (^function 
representing  the  maximum,  applicable,  non-redundant  constraint  from  one  family. 
I'or  each  such  /,  wo  conjecture  that  a  good  potential  field  function  would  be: 


r{x) 


if  f{X)  >  0, 
if  f{X)  0. 


for  some  fc  >  2.  Wlietfier  or  not  such  penalty  fuiuitions  could  bo  used  in  devising 
a  fast  re.il-time  control  algorithm  is,  of  course,  another  ipiestion.  1'he  sugg,('.si ion  is 
[>rimarily  inti'nded  to  show  that  there  is  a  ref)resentation  on  whicli  (in  j)rinci[)le) 
Irs.;  af;{-roxim;ite  potential  field  methods  might  be  based. 
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Moving  Through  Rotation  Space 

5.1.  Introduction 

In  this  chapter  we  discuss  some  of  the  computational  issues  involved  in 
planning  paths  involving  three  dimensional  rotations.  The  primary  issue  is  that 
of  keeping  track  of  which  constraints  (C-functions)  are  applicable  as  orientation 
changes.  In  principle  it  is  possible  to  intersect  paths  with  all  ACF  boundaries,  and 
thus  to  del  ermine  which  applicability  regions  the  path  traverses  and  crosses.  It  is 
also  possible,  in  principle,  to  compute  the  applicability  regions  a  priori,  before  the 
planning  begins.'  In  practice  this  is  computationally  infeasible.  Even  for  simple 
environments,  there  are  typically  thousands  of  constr.aints,  eacli  of  which  has  at 
least  .3  associated  type  (a)  and  (b)  ACh's.  We  will  investigate  alternative  strategies 
which  exploit  coherence  in  how  the  set  of  ap{)licablc  functiotis  changes  as  the 
robot  moves  continuously  through  rotation  space.  In  previous  cha[)lers  (particularly 
chai)tcr  d)  we  showed  how  to  intersect  trajectories  with  C-surfac<'s  and  ACEs. 

d'he  a]>plirnhility  t^el  for  an  orient.ation  0  is  the  set  of  all  applicable  constr.iints 
(C-functions)  there.  (d(uiily,  there  are  r(!gions  on  for  which  the  a[)plicability 

set  is  invariant;  ori(mtations  in  the  interior  of  these  regions  corres()on(l  to  orientations 
where  no  edges  or  faces  f)f  the  robot  arc  aligned  with  the  edges  or  faces  of  any 

'  riiis  a ()|)ro;c(  li  is  ,'ilriiihir  Id  Mic  cril  iral  n  j'iDii  r()[ii|)iil.:Ll  Ioii.h  id  lij  Scliwariz  anil  Sliarir 

(I'lMI). 


obslaclc.  I'or  a  fixcul  orientation  0,  we  compute  the  a[)plicability  set  by  examining 
the  signs  of  the  ACFs  for  all  C-functions  (see  chapter  3).  However,  this  is  clearly 
not  an  operation  we  wish  to  rc[)eat  very  often,  and  the  a{)plicability  set  calculation 
procedure  should  be  rnernoized.  (A  memoized  procedure  records  the  answer  for  a 
given  inf)ut,  so  it  will  not  have  to  be  recom{)uted.  Instead,  it  can  simply  be  looked  up 
in  a  table).  As  the  robot  moves  in  rotation  space,  certain  constraints  will  expire  as 
the  path  moves  out  of  their  applicability  region,  and  other  constraints  will  become 
active  as  we  move  into  their  applicability  region.  This  suggests  that  an  incremental 
update  algorithm  should  be  possible;  we  imagine  detecting  when  constraints  expire, 
and  when  new  constraints  become  active  and  constructing  a  Delelelist  of  expired 
constraints  and  an  Addlist  of  new  constraints.  The  applicability  set  is  then  updated 
by  means  of  the  Deletelist  and  Addlist. 

5.2.  'Phe  Applicability  Dccoinposltion  for  50(3) 

In  this  chapter,  wc  will  first  present  a  naive  algorithm  which  does  not  use 
an  u[)date  strategy.  We  then  present  a  more  sophisticatial  procedure,  called  the 
Q  update  algorithm,  which  is  an  incremental  update  strategy.  We  have  performed 
experiments  using  both  algorithms  to  implement  the  local  operator  Rotate,  which 
was  iliscussed  in  chapter  2.  Doth  algorithms  have  the  same  asymptotic  complexity. 
Although  wc  have  applied  both  to  the  find-path  problem,  they  are  designed  for 
fundamentally  different  tasks.  The  naive  algorithm  is  specialized  for  a  particular 
fiml-path  ojjcrator,  while  the  uprlate  algorithm  is  a  gemual  tool  for  computing  a 
decomposition  of  C-Space  for  spatial  planning. 

'Phe  Q  u{)datc  strategy  addresses  the  fundamental  problem  of  applicability  set 
computations  in  a  continuous  space.  Without  the  Q  algorithm,  there  exists  only 
the  “discrete"  a[)i)licability  set  computation,  which  given  one  point  in  SO{3)  can 
determine  the  set  of  all  applicable  C-functions.  With  an  incrcmiental  update  strategy 
we  can  map  out  regions  on  50(3)  for  which  tin;  appliiaibility  set  is  invariant.  The 
boundaries  of  l.hesc  regions  are  ACF  boundaries.  Let  ]/(0)  be  tin'  applicability 
set  at  0  G  50(3),  and  be  a  binary  relation  on  .S’0(3)  such  that  (-)  ~  O'  if, 
and  only  if  1/(0)  ^  ^  (dearly,  ~  is  an  erpiivalenee  relation  on  50(3),  and 

50(3)  is  decom|)osed  by  ci:  into  disjoint  ('rpiivahmce  classes  where  the  a[)plieability 


set  is  invariant.  Wc  call  this  the  applicability  decomposition  for  50(3).  Computing 
this  decomposition  is  a  fundamental  step  in  reducing  continuous  spatial  planning 
problems  to  discrete  computational  problems. 

We  will  show  how  to  compute  these  decompositions  for  sections  of  50(3)  in 
any  of  the  directions  S  ~  {±.^,±0, In  particular:  The  incremental  update 
strategy  computes  a  projection  of  the  applicability  decomposition  onto  a  subspace 
of  50(3)  which  is  isomorphic  to  5'.  In  principle  it  is  not  hard  to  generalize 
these  sections  to  arbitrary  rotational  slices:  algebraically  tliis  entails  solving  the 
intersection  of  a  TQF  with  an  arbitrary  pure  rotation.  As  wc  have  noticed,  any 
rotation  of  interest  can  be  approximated  as  closely  as  desired  be  a  sequence  of 
rotations  in  S,  with  no  loss  of  completeness  (at  a  given  resolution). 

The  naive  algorithm,  on  the  other  hand,  is  highly  specialized  to  the  particular 
problem  of  rotating  to  a  constraint.  It  does  not  address  the  more  fundamental 
problem  of  decomposing  50(3)  into  applicability  set  equivalence  regions.  We 
believe  that  the  applicability  decomposition  is  also  important  to  planning  problems 
other  than  find-path,  particularly,  for  find-space,  fine-motion,  and  planning  with 
uncertainty.  However,  in  practice  the  naive  algorithm  has  proved  faster  for  rotating 
to  a  constraint  than  any  incremental  algorithm  we  have  devised.  It  is  gratifying  to 
find  that  both  strategies  have  the  same  asymptotic  complexity;  however,  wc  have 
no  strong  indication  that  the  0{N  \og  N)  bound  we  demonstrate  is  optimal,  and 
faster  algorithms  may  exist. 

5.3.  A  Naive  Algorithm  Without  an  Update  Strategy 

Wc  begin  by  presenting  a  naive  algorithm  for  moving  in  rotation  space  which 
docs  not  employ  an  update  strategy.  Wc  wish  to  design  an  effective  procedure  which 
is  to  be  given  a  start  configuration  s,  a  goal  direction  a  G  {  +'tp,  — +0,  —0,  -f^,  — ^  }, 
and  a  goal  configuration  (/.  The  goal  configuration  differs  from  s  only  in  tliat  the 
goal  angle  in  the  a  direction  will  be  y,,  instead  of  .s„.  The  procedure  determines 
if  the  robot  can  reach  g  along  the  trajectory  in  dr,  or  whether  it  will  strike  an 
obstacle,  in  which  case  it  must  return  the  C-surface  hit  and  the  intersection  angle. 
(We  use  intersection  angle  to  mean  the  value  of  a  at  the  intersection  point). 


Let  C  be  the  set  of  all  C-surfaces.  Calculate  the  intersection  of  the  trajectory 

p(f)  =  s  +  ta 

with  every  C-surface  in  C  {whether  applicable  or  not).  Each  such  intersection  can 
be  expressed  as  a  single  angular  value  (i.e.,  the  value  of  t  or  a  for  which  p{t) 
lies  on  the  C-surface)  and  hence  as  a  single  point  on  the  unit  circle.  We  can 
order  the  intersection  points  by  their  intersection  angle  with  a  C-surface.  Sort  the 
intersections  around  the  circle.  Then  traverse  the  intersections  on  the  circle  in 
direction  a  from  s,  and  find  the  first  intersection  which  is  both  applicable  and  on 
the  boundary  of  a  C-Space  obstacle.  In  4.2.2  we  gave  an  algorithm  for  how  this 
may  be  determined. 

5.4.  Update  Strategics:  Example 


We  now  proceed  to  describe  how  an  update  strategy  works.  If  constraints  could 


Figure  5.2.  As  the  hammer  rotates  in  the  —6  direction  from  (V'O)  2.42,  <^o)  t'O  (V’o^O,  ^o),  the  boxes 
in  the  lower  left  show  the  C-Sjiacr.  obstacle  boundaries  and  ACF  l)oundari(-H  that  the  trajectory 
liits.  Since  the  hammer  is  in  rroo-.space,  it  hits  no  C-surfaccs.  However,  it  crosses  many  ACF 
boundaries. 

expire  and  become  active  “arbitrarily”,  this  problem  might  still  be  formidable. 
However  the  following  observation  makes  things  much  easier: 

Claim  (5.1):  When  a  constraint  expires,  another  “neighboring”  constraint 
becomes  active. 

For  example,  consider  figure  5.1,  which  depicts  a  cross-section  of  a  rectangloid  A 
moving  above  an  obstacle  face  /.  As  A  translates,  it  rotates  in  direction  In 
5.1a,  constraint  {vi,f)  is  applicable,  and  {vi,  f)  is  not.  At  5.1b,  however,  we  move 
out  of  the  applicaljility  region  for  (ui,/)  and  {y>,  f)  becomes  active.  5.1b  is  on 
the  boundary  of  the  applicability  regions,  and  both  constraints  arc  applicable.  By 
5.1c,  however,  (u),/)  has  expired.  [v‘i,  f)  has  replaced  [v\,f)  in  the  applicability 
set.  {v‘2,  f)  clearly  seems  like  a  neighboring  constraint  to  {v\,f),  in  that  U]  and  V2 


arc  adjacent  vertices  on  the  edge  graph  of  A.  Wc  would  like  to  devise  an  update 


strategy  which,  given  a  Dclctclist  of  expiring  constraints,  could  enumerate  a  small 
list  of  candidates  for  the  Addlist.  In  general  an  expiring  constraint  will  be  replaced 
by  neighboring  constraints.  However,  the  neighborhood  function  is  somewhat  more 
complicated  than  in  this  simple  example.  For  instance,  imagine  that  A  were  rotating 
towards  the  viewpoint  (out  of  the  page),  leading  with  vertex  u.j  (see  figure  5.4).  It 
is  possible  for  constraints  (v^,/),  and  (v;{, /)  to  replace  (ui,/),  if  the  faces 

/  and  {vi,V2,v^,vn  }  arc  parallel  when  (ui,/)  expires.  Clearly  U3  is  also  “near”  v\, 
but  not  as  near  as  V2  and  vn-  To  exploit  claim  (5.1),  it  remains  to  be  seen  just 
what  we  mean  by  a  “neighboring  constraint.”  We  should  emphasize  that  the  update 
strategy  does  not  predict  exactly  which  constraints  will  become  active,  but  merely 
a  set  of  candidate  constraints,  some  of  which  must  replace  the  expiring  constraints 
in  the  applicability  set. 

5.5.  Using  Update  Strategics 

Let  us  modify  the  naive  algorithm  to  incorporate  an  update  strategy.  At 
configuration  a,  we  compute  the  applicability  set.  The  trajectory  p  is  next  intersected 
with  all  C-surfaccs  in  the  applicability  set,  and  with  all  ACF  boundaries  for  these 
C-surfaces.  The  two  lists  of  intersections  are  merged  and  sorted  around  the  unit 
circle.  (The  sort  key,  once  more,  is  the  intersection  angle).  We  call  this  sorted 
structure  of  C-surface  and  ACF  intersections  the  intersection  queue,  since  it  a 
priority  queue  containing  intersections.  An  entry  in  the  intersection  queue  is  a  pair: 

^C-surface  or  ACF,  Angle  of  intersection 

Wc  then  traverse  the  intersection  queue  in  order  from  s  in  direction  a,  taking 
the  following  actions  when  we  encounter  a  C-surface  or  an  ACF  intersection: 

(1)  When  an  ACF  boundary  is  hit,  a  C-surface  has  expired.  Let  the  angle  of 
intersection  L  a/.  Sometimes  several  C-surfaccs  expire  at  once;  in  this  case  their 
y\CF  boundaries  will  all  have  the  same  intersection  angle  on  the  circle.  Determine 
all  the  C-surfaccs  that  cx]iirc  at  a/  (simply  scan  <lown  the  intersection  queue  until 
an  intersection  angle  greater  than  a/  (with  respect  to  direction  a)  is  found).  These 
C-surfaccs  constitute  the  Dclctclist.  Assume  we  have  an  update  procedure,  which 
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can  determine  an  Addlist  of  newly  active  C-surfaces  given  a  Deletelist  of  expiring 
C-surfaces.  Call  the  update  procedure  with  the  Deletelist,  to  determine  the  Addlist. 

(i)  Delete  all  C-surfaces  in  the  Deletelist  from  the  Applicability  set. 

(ii)  Delete  all  C-surfaces  in  the  Deletelist  from  the  intersection  queue. 

(iii)  Delete  all  the  ACI'^s  of  C-surfaces  in  the  Deletelist  from  the  intersection  queue. 

(iv)  Create  an  Addlist  intersection  queue,  i.c.,  a  sorted  structure  containing  the 
intersections  of  all  C-surfaces  in  the  Addlist,  and  all  A(/F  boundaries  of  these 
C-surfaccs,  with  the  trajectory  p. 

(v)  Merge  the  Addlist  intersection  queue  with  the  old  intersection  queue. 

(2)  When  encountering  a  C-surface  intersection,  we  know  the  C-surface  must 
be  applicable,  since  we  have  not  yet  hit  an  '  nindary  which  could  invalidate  it. 

(This  is  essentially  tlu;  correctness  criterion  maintained  by  step  (1)  of  the  algorithm). 
Test  to  see  if  the  intersection  is  on  the  boundary  of  the  C-Space  obstacle.  Note 
that  this  operation  typically  requires  knowing  the  applicability  set. 

We  then  continue  traversing  the  intersection  queue  (of  course,  resuming 
traversal  the  next  a  slightly  beyond  a/  in  the  (\  direction)  until  cither  an  obstacle 
is  hit  or  the  goal  angle  is  reacherl.  As  the  intersection  queue  is  travers(Hl,  steps 
(1)  and  (2)  are  performed  to  update  the  queue  and  detect  collisions  whenever  an 
ACF-boundary  or  C-snrfacc  (respectively)  is  crossed.  is  typically  quite  dense 

in  ACF  boundaries;  sec  figure  5.2.  In  this  figure,  the  small  boxes  depict  one 
dimensional  slices  (isomorphic  to  5’)  of  rotation  space  in  the  —0  direction.  The 
thin  line  extending  out  of  the  circle  indicates  the  start  angle,  which  is  0  —  2. -12 
radians,  and  the  heavy  line  extending  out  of  the  circle  indicates  the  goal  angle, 
which  is  —  0.  The  intersections  of  the  trajectory  with  C-surfaces  are  shown  in  the 
left  box  (there  are  none).  The  intersections  of  the  trajectory  with  ACF  boundaries 
are  shown  in  the  right  box.  I'kich  line  indicates  the  angle  of  intersection  for  an 
ACF  boundary.  The  apiilicability  set  is  invariant  between  intersection  points.  The 
moving  object  is  shown  rotating  between  the  start  and  goal  angle.  The  C-surfaces 
and  ACl's  were  generated  by  the  moving  object  and  obstacles  shown.  However,  the 
actual  size  of  each  Addlist  is  usually  small.  The  algorithm  works  by  maintaining  a 
correct  apjilicability  set  as  we  move  in  a,  and  by  modifying  the  intersection  queue 
to  remove  C-surfacc  and  ACF  intersections  that  arc  not  applicable. 


5.6.  Update;  Strategies 


In  this  section  we  finally  discuss  specific  update  strategies.  An  update  strategy 
has  two  parts:  first,  given  a  Deletelist  of  expiring  constraints,  it  must  predict  a 
set  of  C-functions  guaranteed  to  contain  the  Addlist.  Second,  it  must  test  each  of 
these  predictions  to  determine  which  are  really  applicable.  The  latter  operation  is 
conceptually  trivial,  but  since  it  is  expensive,  we  wish  to  make  the  prediction  set 
as  small  as  possible.  For  example,  predicting  C,  the  set  of  all  C-functions  is  clearly 
correct,  but  not  very  useful. 

A  better  approximation  would  be  as  follows:  given  a  Deletelist,  determine 
all  the  C-families  (i.e.,  families  of  C-surfaces)  it  represents.  A  safe  prediction 
would  comprise  all  the  C-functions  in  these  families,  since  clearly  an  expiring 
constraint  will  be  replaced  by  another  constraint  from  its  own  family.  In  practice 
this  approximation  has  proved  useful,  however,  it  is  not  the  best  we  can  do.  In 
particular,  note  that  even  two  cuboids  will  generate  d8  type  (a)  C-surfaces,  48  type 
(b)  C-surfaces,  and  144  type  (c)  C-surfaces.  Clearly  the  C-family  approximation  is 
not  a  very  tight  upper  bound  for  the  replacement  set,  that  is,  the  Addlist  for  a 
Deletelist. 

Let  T/',  (fp,  and  //>  denote  the  vertices,  edges,  and  faces  of  polyhedron  P.  For 
a  moving  polyhedron  A  and  an  obst.acle  polyhedron  B,  we  can  express  the  family 
of  constraints  as: 


[Ja  X  X  X 

lb  be  formal,  this  should,  strictly  speaking,  be  considered  the  domain  of  a  bijection 
C  which  maps  pnirs  of  generators  to  the  function  space  of  C-functions,  but 
where  there  is  no  ambiguity  we  will  speak  of  a  pair  [gA,Qii)  as  representing  the 
corresponding  C-function  C{gA,gn)- 

For  an  expiring  C-function  {g,\,gn),  we  would  like  to  define  a  neighborhood 
map  on  a  polyhedron  P, 
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(where  *  is  the  Klcene  star  denoting  closure)  such  that  the  set 

9i9A)  X  9i9n)  (5.1) 

is  the  smallest  maximal  replacement  set  for  {gAtSn)-  In  other  words,  we  want  (5.1) 
to  contain  all  possible  replacement  sets  for  (g/i,<7/j),  no  matter  what  the  rotational 
motion;  but  we  also  wish  (5.1)  to  be  as  small  as  possible  so  as  to  minimize  the 
ACF  computations.  It  is  possible  for  Q  to  be  local  in  character:  although  several 
constraints  in  a  family  may  expire  simultaneously,  all  that  we  require  is  that  the 
union  of  their  replacement  sets  is  correct. 

We  conjecture  it  migi  .  be  possible  to  find  exact-or  at  least  smaller  replacement 
sets  by  taking  the  specific  motion  into  account.  Such  a  strategy  has  not  yet  been 
developed,  however. 

5.6.1.  Mathematical  Preliminaries 

In  chapter  3,  we  gave  an  informal  definition  (by  example)  of  the  boundary  and 
coboundary  operators.  We  now  define  and  employ  two  related  operators  which  can 
be  composed  to  define  operators  such  as  “the  faces  which  contain  vertices  iq,  V2, 
and  ii;j”  and  “the  edges  which  are  incident  at  the  vertices  of  these  faces.” 

In  this  section  we  define  the  discrete  boundary  and  coboundary  operators. 
Consider  a  finite  collection  of  cells,  S.  The  discrete  boundary  And  discrete  coboundary 
of  S,  denoted  OS  and  6S,  are  defined  as  follows: 

dS  =  u  as 

sQS 

6S  =  U  6s. 

.les 

The  discrete  boundary  and  coboundary  operators  have  very  different  properties 

from  the  normal  boundary  and  coboundary  operators.  For  example,  if  /  is  a  face, 

„  . 

then  d~f  —  0,  while  Of  —  vcri[f).  To  see  this,  observe  that 


jLa _ .  ■■ . ^9. 


a  1=^  d{dj) 

=  \J  de 

rebf 

—  vcrt(f). 

In  fact,  for  any  “well  behaved”  object  P  (and  in  particular,  any  polytope),  d^P  =  0 
and  S^P  =  0  (this  is  a  fundamental  theorem  of  topology).  However,  two  (or  more) 
applications  of  the  discrete  boundary  or  coboundary  operator  will  not,  in  general, 
yield  0. 

Examples:  6  {vi,V2,  v-^)  is  the  set  of  faces  F  which  contain  at  least  one  of  the  vertices 

a2  -'2  a2a2 

v\,  v-i  or  U3.  Since  for  one  face  f,  d  f  =  vert{f),  then  B  F  =  d  6  (ui, i;2, u.i) 
is  the  vertices  of  all  the  faces  F.  The  set  of  edges  incident  at  these  vertices  is 
6d^6^{vi,V‘2,v-i). 

«2-3 

Exercise-.  What  is  5  6  (nj ,  i;2,  «;<)? 

IClcmcritary  Review:  Houndary,  Coboundary,  and  Star 

We  must  show  that  the  discrete  boundary  and  coboundary  operators  are  well 
behaved.  We  will  do  so  by  presenting  a  formal  definition  of  d  (and  6)  on  a  single 
chain.  Readers  who  have  encountered  a  bit  of  homology  will  find  the  demonstration 
transparent.  Others  may  wish  to  take  this  section  on  faith,  and  to  skip  to  the  next 
section,  where  we  define  the  star  operator. 

Discrete  boundary  and  coboundary  operators  can  be  considered  as  the  ordinary 
boundary  and  coboundary  “modulo  orientation.”  We  see  this  as  follows.  (For  a 
more  comprehensive  account  see  any  textbook  on  elementary  topology,  for  example. 
Hocking  and  Young  (1961)). 

Let  K  be  an  arbitrary  oriented  complex  of  abstract  cells,  and  Z  an  arbitrary 
(additivcly  written)  abelian  group.  An  n-dimcnsional  chain  on  the  complex  K  with 
coellicients  in  Z  is  a  function  c,,  majiping  oriented  n-cells  of  K  lo  Z ,  such  that  if 
—  z,  then  r„(-  a")  —  -z.  An  arbitrary  n-chain  c„  on  K  can  be  written  as 
the  formal  linear  combination 
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where  z,  =  c„(+(7").  The  boundary  operator  d  is  a  mapping  From  n-chains  to 
(n  —  l)-chains.  5(z,  •  a")  is  an  (n  —  l)-chain  which  has  non-zero  coelFicients  only  on 
the  (n  —  l)-faces  of  the  cell  or".  Formally,  let  ']  be  the  incidence  number  for 

ct"  and  or"  that  is 


fo, 

+1, 

l-i, 


if  <7""’  is  not  a  face  of  a", 

if  or”"'  is  a  positively-oriented  face  of  <7”, 

if  o”"’  is  a  negatively-oriented  face  of  ct”. 


Hence, 


To  factor  out  the  effect  of  orientation,  we  dcGnc  the  discrete  boundary  operator  as 
follows; 


‘• 

CT"-' 

Discrete  coboundary  is  defined  analogously. 

The  Star  Operator 

I^et  P  be  a  polyhedron.  Any  cell  k  is  a  face  of  itself,  although  is  not  a  proper 
face.  A  proper  face  of  f’  must  be  lower  in  dimension  than  If  an  n-dirnensional 
cell  k  is  on  the  boundary  of  P,  then  we  call  k  an  proper  n-face  of  P.  Thus  edges 
are  proper  l-face.s,  and  vertices  pro[)er  0-faces  of  a  d  dimensional  polyhedron.  Let 
K  be  some  complex  of  cells.  If  k  is  a  n-face  of  K,  then  we  write  AT  >  k.  We  will 
usually  assume  that  a  face  is  a  projier  face. 

Now,  let  L  be  some  set  of  cells  in  K .  The  star  of  LI  (in  1C)  is  defined  by 


ISf) 


St(E,  /C)  =  {  a  G  /C  I  (3  r  €  E),  a  >  r  }, 

i.e.,  the  set  of  all  cells  in  K  that  contain  a  niernber  of  E  in  their  boundary.  When 
there  is  no  ambiguity  we  will  simply  write  St(E).  (Giblin  (1977),  Hocking  and  Young 
(1961)). 

For  a  cell  k,  define  6  k  =  k,  6  k  =  6k,  and  6  k  =  6(6k),  (etc).  We  see 
immediately  that  the  star  of  {  k  }  may  be  computed  as 


St({k})=  U  6  k. 

t=0 

Using  this  observation,  we  have  implemented  the  star  operator  by  recording  the 
boundary  and  coboundary  of  each  cell  in  the  geometric  model. 

5.6.2.  Local  Cornpiit.ation  of  Ronlaccmcnt  .Sets 
Type  (a)  and  (b)  Constraints 


Consider  figure  5.3.  (ui ,  /)  denotes  a  type  (b)  constraint.  Consider  any  rotational 
motion  from  the  configuration  shown.  Assume  this  rotation  will  cause  {v[,f)  to 
expire.  We  wish  to  determine  the  maximal  possible  type  (b)  rciilaceinent  set  for 
(u|,/),  that  is,  the  set  of  neighboring  type  (b)  constraints  which  could  replace  (ui , /) 
under  any  conceivable  rotation. 


Consider  the  set 


^d(6ui)  -  {n|  X  {/}. 


htJx  — :  6v\  is  just  the  edges  which  meet  at  v\.  The  discrete  boundary  of  these 
I'dges  is  simply  the  collection  of  their  vertices.  i;i  is  deleted,  since  it  is  expiring. 
.\ow,  consider  a  rotatioii.il  motion  which  causes  (u),/)  to  expire.  (5.2)  will  cont.iin 
replacement  tyjie  (b)  const  rainis.  However.  (5.2)  is  not  maximal:  consider  a  rot  ation 
which  causes  (n|,/)  to  ex|)ire  at  some  orientation  at  which  a  face  f'  containing  ui 
is  [i.'ir.ille!  to  /.  (See  figure  .5.1).  'fhen  .ill  the  vertices  of /'  are  re|)l;icemenls  for  Ui, 


that  is,  the  type  (b)  replacement  set  is  ^verl{f')  —  {  ui  }  j  X  {  /  }•  In  general,  the 
maximal  predicted  type  (b)  replacement  set  for  (vi,/)  is 

(verl{6^vi)  ~  {vi}^  X  {f  }.  (5.3) 

By  similar  analysis,  we  see  the  following  claims: 

Claim  (5.S):  When  (t'l,/)  expires,  so  will  at  least  one  type  (c)  constraint  with 
generators  in 

Ivi  X  Of. 

Claim  (5.4):  Sec  figure  5.5.  When  a  type  (c)  constraint  {ea,^b)  expires,  so  must 
some  ‘ype  (b)  constraint  with  generators  in 
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that  is,  ^(k)  is  the  set  of  all  cells  which  contain  vertices  of  k  as  faces. 


Let  P  be  a  Deletclisl.  The  smallest  maximal  replacement  set  for  P  is 

U  9[9a)  X  ${gn). 

{gA,gii)£D 

This  particular  formulation  requires  that  we  ignore  “nonsense”  pairings  such 
as  all  members  of  Vx  X  Vu-  This  is  easily  accomplished  by  appropriate  construction 
of  the  function  C  mapping  pairs  of  generators  to  the  function  space  of  C-functions. 

A  A 

We  extend  the  domain  of  C  to  dA  X  dD,  and  map  all  generator  pairs  except  those 
in  (/a  X  V/,)U(‘V^  X  X  Sn)  to  0. 


A  Correctness  Proof  for  Q 


We  shall  now  argue  that  Q  is  the  correct  mapping  to  predict  smallest  maximal 
replacement  sets.  Let  {gAtQn)  be  an  expiring  constraint  at  some  orientation  0.  The 
replacement  set  for  a  constraint  is  the  Cartesian  product  of  the  the  replacement 
sets  for  its  generators.  Hence  the  the  replacement  set  for  {gA,gn)  is  X  r(gif). 

Consider  the  replacement  set  r^g^)  for  g^t  i-e-»  the  set  of  constraints 


r{gA)  X  {  ff/r } 

which  will  replace  This  set  of  constraints  will  become  active  at  orientation 

0,  while  gii)  expires.  Let 


G  =  {gyilUKa/i)- 

iNotc  tliat  (1)  all  constraints  in  C  X  {  gn  }  must  be  applicable  at  orientation  0,  and 
(2)  0  must  lie  on  the  boundary  of  each  of  their  applicability  regions  in  SO(3).  Wc 
say  that  at  0  each  g  €  G  is  in  ACF  boundary  condition.  Note  further  that  r{gA) 
is  not  a  predictive  replacement  set,  but  any  actual  replacement  set  for  a  generator 
gA  under  some  arbitrary  rotation. 

Wc  will  first  show  that  all  €  G  arc  coplanar. 

All  ACFs  arc  defined  in  terms  of  a  contact  vertex  and  an  applicability  vertex 
(see  chapter  3).  The  contact  vertex  is  brought  to  rest  on  some  applicability  plane 
(which  is  parallel  to  a  face  of  the  other  polyhedron),  and  the  applicability  vertex 
is  constrained  to  lie  above  that  plane.  When  a  constraint  is  in  ACF  boundary 
condition,  then  both  the  cont.act  vertex  and  the  applicability  vertex  of  at  least  one 
of  its  ACFs  are  constrained  to  lie  on  the  applicability  plane.  (We  consider  type  (c) 
constraints  to  be  composed  of  four  such  ACFs,  two  of  which  arc  type  (a)  and  two 
or  which  are  type  (b)  ACFs).  In  addition,  observe  that  each  line  segment 


Applicability  vertex.  Contact  vertex 


lies  on  some  edge  of  cither  polyhedron  A  or  polyhedron  B.  In  fact,  these  edges, 
which  we  term  applicability  edges,  cover  the  edge  graphs  of  both  polyhcdra,  although 
the  mapping  is  many-one.  We  are  given  a  generator  g/\  for  a  constraint  {gA,gn)- 
The  constraint  is  placed  in  ACF  boundary  condition.  This  requires  aligning  an 
(applicability)  edge  of  A  with  a  face  of  B  (or  vice  versa).  (This  point  is  fundamental 
to  understanding  the  correctness  argument:  if  both  contact  vertex  and  applicability 
vertex  must  lie  on  the  applicability  plane,  then  the  applicability  edge,  which  is 
an  actual  edge  of  A,  must  be  aligned  with  the  plane).  We  are  then  asked  to  find 
all  constraints  which  can  be  simultaneously  placed  in  ACF  boundary  condition. 
This  is  equivalent  to  asking,  “Given  one  edge  of  A  aligned  with  some  face  of 
B,  and  maintaining  this  alignment,  what  additional  edges  can  simultaneously  be 
aligned  with  faces  of  B,  such  that  all  associated  constraints  are  in  ACF  boundary 
condition?”  (By  associated  constraints  we  mean  the  following;  the  aligned  edge  is 
considered  as  an  applicability  edge.  Since  the  applicability  edges  cover  the  edges 
of  the  polyhedra,  the  associated  constraints  for  an  applicability  edge  e^  are  those 
C-functions  for  which  the  orientation  of  Cy^  determines  applicability.) 

Now,  by  fixing  an  edge  e^  at  some  arbitrary  aligned  orientation  (with  a 
face  of  B),  we  retain  one  rotational  degree  of  freedom  ip  about  e^-  We  wish  to 
choose  this  rotation  such  that  (1)  the  constraint  associated  with  e^  (i.e.,  {gA,gn)) 
remains  applicable  (and  of  course,  in  boundary  condition),  and  (2)  a  maximal  set 
of  constraints  is  simultaneously  placed  in  boundary  condition.  The  replacement  set 
we  compute  is  the  union  of  these  maximal  sets.  (2)  requires  a  maximal  number  of 
additional  edge  alignments,  and  must  also  preserve  the  disjoint  interior  criterion. 
So  choosing  p  so  as  to  maximize  the  number  of  edge  alignments  propagates  the 
alignment  constraint.  Clearly,  by  propagating  the  alignment  constraint,  wo  obtain 
a  set  of  coplanar  edges  (recall  that  A  and  B  are  convex).  Each  edge  represents  a 
contact  vertex  and  an  applicability  vertex  for  (one  or  more)  ACFs  in  applicability 
boundary  condition.  The  associated  generators  must  also  be  coplanar. 

We  have  .seen  that  all  replacement  generators  r[gA)  must  be  coplanar  with 
gA-  (As  usual,  there  exists  a  symmetric  argument  for  r(g/j)).  Given  an  expiring 
generator  gi>  on  a  polyhedron  P,  we  wish  to  predict  replacement  sets.  Replacement 


sets  arc  obtained  from  maximal  sets  of  coplanar  generators  which  contain  vertices 
of  gi>.  Clearly,  the  maximal  coplanar  sets  of  generators  for  a  convex  polyhedron 
are  exactly  its  faces  (and  their  boundaries).  Hence,  to  predict  replacement  sets,  we 
must  find  the  set  of  faces  of  P  (along  with  their  boundaries]  that  contain  vertices 
of  gi>.  This  set  is 

5{gi>)  =  Sl{vert(g/>),  dP).  | 

5.7.  Analysis  and  Evaluation 

We  have  implemented  algorithms  for  moving  in  some  selected  rotational 
direction  until  cither  the  goal  or  a  C-surfacc  is  reached:  The  naive  algorithm  (see 
section  5.3),  the  predictive  update  algorithm  based  on  the  C-surface  family  as  a 
loose  maximal  bound  on  the  replacement  set  (section  5.6),  and  the  incremental 
update  algorithm  based  on  Q  (section  5.6.3).  We  next  show  that  the  naive  algorithm 
and  the  ^  algorithm  both  have  the  same  asymptotic  complexity.  This  moans  that 
their  performance  will  largely  Hepend  on  the  constant  factors  in  the  computation. 
We  discuss  empirical  results  to  indicate  the  size  of  these  constants. 

5.7.1.  Complexity 

Naive  Algorithm:  O(7ilogn) 

Let  N  bo  the  fiumbor  of  C-surfaces  in  the  environment  (including  non- applicable 
C-surfaces).  If  the  moving  object  is  made  up  of  mo  convex  polyhedra  with  ko 
generators  each,  an<l  the  obstacle  environment  comprises  no  convex  polyhedra 
with  jo  generators  each,  then  clearly  N  —  jokamano-  The  complexity  of  the  naive 
algorithm  is  .as  follows; 

(i)  Intersect  trajectory  with  all  C-surf.aces  (0(/V)). 

(ii)  Sort  intersections  aroutid  5'.  (0(/V  log  TV)). 

(iii)  For  each  intersection,  determine  if  it  is  applicable  and  on  the  boundary  of  a 
C-Sj>ar.c  ob.stacic.  I'ir.st,  lest  to  .see  if  the  ('-sijrf.ice  is  applicable  by  examining 
its  A(3''s.  If  so,  there  are  two  options:  (1)  if  the  applicability  set  is  known  at  the 
intersection  point,  we  can  test  to  see  if  the  other  C-functiofis  in  the  family  .are 
negative  or  zero.  (2)  If  the  applicability  set  is  not  known  .at  the  intersection  point, 
we  can  compute  the  disfilaced  face  of  the  Minkowski  solid  corresponding  to  the 
two  generators  for  the  (’-surface.  Next,  test  to  see  whether  the  intersection  point 
falls  within  the  face.  (1)  would  make  this  step  (^^((j()A:o)"m()7i())  =  0{j\)ki)N). 


However,  (2)  needs  only  examine  the  generators  of  a  constraint,  and  allows  this 
step  to  be  0{N).  {0{N)). 

We  see  that  the  complexity  of  the  naive  algorithm  is  0{N  log  N). 

Q  Update  Algorithm:  0{N  log  N) 

The  complexity  of  the  Q  update  algorithm  is  as  follows: 

(i)  Intersect  trajectory  with  all  applicable  C-surfaccs  and  their  ACFs.  Let  the 
number  of  applicable  C-surfaces  be  y  <  N,  and  the  number  of  ACFs  per 
C-surface  be  a.  (y(l  +  a)  intersections  =  0(^)). 

(ii)  Sort  the  intersections  around  5*.  (0(^  log  ^)). 

(iii)  For  each  intersection:  Sort,  add,  and  delete  j  C-surfaces  from  the  intersection 
queue.  (0(y  logj)). 

This  yields  complexity: 


N 

k 


log  ^  j  log  y  =  ^  (log  N  -  log  A:)  -f  ^  (j  log  j) 


^0{N  logN). 


In  the  next  section,  we  justify  treating  A;  as  a  constant. 


Actual  Performance 

In  practice,  the  naive  algorithm  has  run  faster  than  the  ^  update  algorithm 
for  the  specific  problem  of  rotating  until  a  C-surface  (or  the  goal)  is  reached.  As  the 
complexity  analysis  has  shown,  since  both  algorithms  arc  0{N  log  N),  the  difference 
in  performance  will  be  due  to  different  constant  factors.  A  good  estimate  for  fc  is 
10.  For  example,  in  a  typical  environment  with  624  type  (a),  704  type  (b),  and  1872 
type  (c)  C-surfaces,  4  sample  applicability  sets  have  sizes  353,  362,  365,  and  355.  j 
is  quite  small;  for  this  environment  it  is  typically  between  2  and  40.  The  number  of 
ACFs  per  C-surface  depends  on  the  degree  of  the  vertices.  For  trihedral  vertices, 
for  example,  a  <  4  (type  (c)  C-surfaces  have  4  ACFs).  Hence  this  tends  to  balance 
out  any  possible  gains,  since  k  is  not  much  bigger  than  a. 


Once  more  we  should  remember  that  the  p  update  algorithm  is  designed  to 
solve  the  more  general  problem  of  applicability  decomposition  of  50(3),  while  the 
naive  algorithm  has  been  specialized  to  solve  the  “rotate  to  a  C-surface”  problem. 


We  speculate  that  similar  specialized  algorithms  may  be  developed  as  fast  solutions 
to  speciGc  spatial  planning  problems.  However,  decomposition  tools  arc  a  more 
general  solution  which  can  be  applied  to  a  whole  class  of  spatial  planning  problems. 

5.7.2.  Related  Work,  Searching  and  Lazy  Evaluation 

The  implemented  planning  system  is  described  in  chapter  2.  The  control 
structure  of  the  algorithm  is  a  search.  The  search  employs  certain  local  operators 
for  moving  between  configurations.  One  such  local  operator  is  precisely  the  "rotate 
to  a  C-surface  (or  the  goal)”  algorithm. 

As  for  most  heuristic^  search  algorithms,  an  adversary  can  probably  devise  a 
find-path  problem  which  must  require  an  exponential  amount  of  time  to  solve.  This 
does  not  imply  that  a  polynomial- time  algorithm  using  the  mathematics  presented 
in  this  thesis  could  not  be  devised;  indeed,  the  theoretical  work  of  Schwartz 
and  Sharir  (1982a)  suggests  this  possibility.  However,  in  practice,  the  planner  has 
performed  quite  well.  We  offer  the  following  explanation  for  why  the  planner  should, 
in  average  cases,  perform  better  than  in  the  adversary  situation. 

In  the  theoretical  work  of  Schwartz  and  Sharir  (1982a)  and  Schwartz  and 
Sharir  (1981),  the  concept  of  non- critical  regions  is  introduced.  A  non-critical 
region,  intuitively  speaking,  is  a  region  in  (free)  configuration  space  where  the 
constraints  are  invariant.  We  employ  similar  constructs  via  sets  of  non-redundant 
constraints,  and  by  means  of  applicability  sets.  In  Schwartz  and  Sharir  (1981),  for 
example,  free  space  is  decomposed  into  critical  and  non-critical  regions,  and  the 
connectivity  of  these  regions  computed.  The  connectivity  graph  is  then  searched 
for  a  path.  However,  computing  these  regions  is  (geometrically  and  algebraically) 
quite  difficult.  The  regions  are  at  least  as  complex  as  the  C-Space  obstacle  and 
applicability  regions. 

Instead  of  precomputing  the  applicability  regions  (or  knowing  them  a  priori), 
our  planner  computes  them  as  it  explores  configuration  space.  While  in  the  worst 
case  the  entire  applicability  decomposition  must  be  calculated,  this  case  docs  not 

*Wc  iiai;  tin;  U;rtn  heuristic  in  reference;  Ui  the  time  complexity,  .and  not  the  completeness  of  the 
algorithm. 


6 

The  C-Voronoi  Diagram  and  its  Relationship 

to  Intersection  Manifolds 

6.1.  Introduction 

For  a  finite  set  of  points  P  in  the  plane,  the  Voronoi  diagram  is  the  set  of  all 
points  in  the  plane  which  are  equidistant  from  two  or  more  points  in  P.  The  Voronoi 
diagram  for  P  is  a  network  of  str.aight  line  segments.  Drysdalc  (1983)  introduced 
the  generalized  Voronoi  Diagram  (or  GVD)  for  the  plane:  for  a  set  of  polygons  in  the 
plane,  the  GVD  is  defined  to  be  all  points  in  the  plane  which  lie  (perpendicularly) 
erjuidistant  between  two  or  more  polygons.  The  GVD  is  a  network  of  straight  line 
segments  and  parabolic  sections.  If  the  polygons  are  considered  as  obstacles,  the 
GVD  represents  the  network  of  paths  through  frce-space  which  maximize  clearance 
from  the  obstacles.  Drooks  (tOS3a.)  and  (VDunlaing  and  Yap  (19S2),  O’Dunlaing, 
Sharir  .and  Yap  (1982)  have  developed  definitions  and  algorithms  employing  an 
extension  of  the  Voronoi  di.agram  for  low-<limensional  configuration  spaces.  Nguyen 
(1983)  also  discusses  the  relationship  of  global  methods  to  the  GVD. 

More  formally,  the  generalized  Voronoi  di.agram  (and  its  extensions)  decompose 
the  free  space  into  a  set  of  regions,  {  /?, },  such  (hat  all  points  X  G  Hi  are  closer  to 
one  obst.acle  than  to  any  other.  Thus  points  on  the  GVD  arc  equidistant  from  two 
or  more  obstacles. 


Figure  6.1.  A  picture  of  the  generalized  Voronoi  Diagram  for  a  bounded  2D  workspace  containing 
four  polygonal  obstacles.  Reprinted  with  permission  from  Nguyen  (1083). 

In  this  chapter,  we  extend  the  concept  of  the  generalized  Voronoi  diagram  to  the 
six  dimensional  C~Space  5R''  X  50(3),  to  provide  a  formal,  constructive  definition 
of  the  C- Voronoi  Diagram,  or  CVD.  The  CVD  is  an  attractive  construction,  in  that 
it  contains  a  representative  component  for  each  “branch”  of  free  space.  Each  such 
component  is  submanifold  of  dimension  0  <  d  <  5,  called  a  Voronoi  manifold.  We 
will  derive  the  following  connection  between  intersection  manifolds  and  the  CVD: 

Let  p  be  a  path  along  the  CVD.  p  lies  along  a  connected  chain  of  Voronoi 
manifolds,  Vi , . . . ,  "V*  •  We  demonstrate  that  for  each  Voronoi  manifold  V,,  there 
exists  an  equivalent  intersection  manifold  of  level  C-surfaces,  J,-.  Furthermore, 
we  also  show  that  for  every  connected  chain  of  Voronoi  manifolds,  there  is  an 
equivalent  connected  chain  of  intersection  manifolds  (of  level  C-surfaces).  (The 
equivalence  we  demonstrate  is  actually  stronger  that  homotopic  equivalence). 

This  yields  an  immediate  (theoretical)  completcne.ss  result  for  planning  along 


E 


intersection  manifolds.  While  our  proof  is  constructive,  it  cannot  be  considered  an 
effective  procedure.  The  charts  for  the  Voronoi  manifolds  are  undoubtedly  very 
difficult  to  derive.  This  in  turn  makes  it  hard  to  develop  planning  algorithms  along 
the  C- Voronoi  diagram.  In  C-Space,  the  most  attractive  feature  of  the  CVD  is  not 
that  it  maximizes  clearance  from  obstacles,  but  that  it  represents  the  connectivity 
of  free  space.  In  other  words,  given  the  CVD,  the  Movers’  problem  can  be  solved 
by  connecting  the  start  and  goal  configurations  to  the  same  connected  component 
of  the  CVD.  But  since  the  Movers’  problem  has  already  been  reduced  to  the  task  of 
navigating  a  point,  it  is  clear  that,  modulo  some  uncertainty  bound,  we  do  not  need 
to  maximize  clearances  while  in  planning  paths  in  C-Space.  We  demonstrate  that 
instead,  it  is  possible,  in  principle,  to  devise  a  planning  algorithm  along  intersection 
maniiolds — for  which  we  have  derived  charts  (chapter  4)-— which  is  equivalent  to  a 
planner  along  the  CVD. 

Generalized  Voronoi  Manifolds 

In  this  section  we  define  the  C-'  oronoi  Diagram  {CVD)  for  the  configuration 
space  X  5G(3).  Note  that  for  liiuchdean  configuration  spaces  we  would  employ 
the  standard  techniques  (Drysdalc  (1983)).  The  metric  in  X  -S'0(3)  is  non-obvious, 
and  the  CVD  does  not  reduce  to  the  CVD  when  rotations  are  factored  out.  However, 
it  has  the  same  connectivity  as  the  CVD.  The  CV D  for  configuration  spaces  without 
a  Euclidean  distance  metric  is  fundamentally  different,  and  is  defined  as  follows. 

To  define  the  CVD,  we  rely  on  the  collection  of  pseudo- metrics  provided  by 
the  g«;onietric  interpretation  of  C-function  values  (chapter  3).  Intuitively,  within 
some  well-defined  region  in  free-space  whore  a  C-function  is  non-redundant,  its 
value  ( liaraeterizes  the  translational  distance  to  either  (l)  an  obstacle  face,  or  (2) 
live  plane  of  the  obstacle  face.  Formally: 

In  this  chapter,  we  will  use  J  C  3?^  X  50(3)  to  denote  free  space.  See  chapter 
2  for  a  formal  review  of  charts  and  atlases.  As  noted  in  more  dct.ail  in  chapter  2,  in 
this  thesis  we  usually  specify  charts  via  the  inverse  form  h  :  E''  — ►  M  (where  E’'  is 
an  open  subset  of  0?")  with  the  understanding  that  it  is  the  inverse  (or  set  of  local 
inverses)  h.  '  which  provides  the  family  of  charts  {(h“',W,)},  for  Ui  W'i  —  h{E^). 


Definition  (I):  Lei  A1  be  the  set  of  families  of  C-functions  on  3?'*  X  ^>’0(3).  For 
^  €  /)  let  Ax  be  the  set  of  maximum,  applicable,  non-redundant  C-futictions 
within  families,  that  is,  if  M  €  M  is  a  family  of  C-functions,  and  M'  Q  M  is 
the  subset  of  applicable  and  non-redundant  C-functions  at  X,  then  M  contributes 
to  Ax  the  function  f  E  M'  such  that  f{X)  >  h{X)  for  all  /i  G  M'  —  {  /  }.  If  /c 
functions  in  M'  tic  for  maximum,  then  M  contributes  all  k  to  Ax- 

Let  n  be  the  dimension  of  C-Space.  Now,  X  C  CVD  if  there  exists  a  maximal 
subset  D  of  Ax,  containing  at  least  two  and  no  more  than  n  C-functions,  such  that 
all  functions  in  B  have  the  same  value  6o  at  X  and  all  functions  in  Ax  —  B  have  a 
value  greater  than  6o-  We  say  that  X  €  CVD  lies  on  an  (n  —  |i?|  -f  l)-dimcnsional 
Voronoi  Manifold.  The  C-Voronoi  Diagram  for  C-Space  is  the  union  of  these 
Voronoi  Manifolds. 

We  have  seen  that  a  level  C-manifold  is  analogous  to  a  level  surface  in  in 
that  it  is  the  set  of  configurations  {  X  |  f{X)  =  for  some  applicable  C-function 
/.  Clearly,  points  on  a  A;-dimensional  Voronoi  manifold  “V  lie  on  the  intersection  of 
n  —  k  1  equal  level  C-manifolds,  i.e., 

/,(X)  =  -..  (X)  =  £(X) 

where  the  level  ^(X)  is  allowed  to  vary  as  X  moves  along  V.  Furthermore,  wc  insist 
that  the  C-functions  /,  constructing  the  Voronoi  manifold  must  belong  to  pairwise 
distinct  families. 

When  we  say  that  a  Voronoi  manifold  "V,-  is  constructed  from  a  set  of  constraints 
F,-,  we  mean  that  all  the  C-functions  /  C  F^  have  equal  value  along  "V,.  An  intersection 
manifold  constructed  from  F,  is  the  intersection  of  level  C-surfaccs  for  constraints 
in  lly  this  wc  mean  that  first  a  level  is  chosen  for  each  /  C  F,,  and  then 
the  resulting  level  C-surfaccs  arc  intersected.  In  general,  a  level  C-surfacc  for  a 
C-function  /  at  level  f  h.as  the  form 

/  V)- 

Ml!) 


/“*(^)  is  the  trivial  intersection  manifold,  that  is,  the  manifold  constructed  by 
intersecting  one  level  C-surface  with  itself.  The  intersection  of  two  level  C-surfaces 
is 


The  intersection  manifold  1[  is  constructed  from  the  same  C-functions  at  iitferent 
levels: 


r.'  =  r'(<l)nr'(®- 


An  Overview  of  the  Proofs 


Let  p  be  a  path  along  the  CVD.  p  lies  on  a  connected  chain  of  Voronoi 
manifolds.  Call  this  chain  For  a  Voronoi  manifold  V,  we  say  that  an 

intersection  manifold  J,  is  equivalent  to  "V,-  if  (1)  the  set  of  C-functions  which 
construct  V,  is  a  (possibly  non-strict)  superset  of  the  C-fiinctions  F-  which  define 
(2)  7,-  is  homotopically  equivalent  to  "V,-,  (3)  7j  lies  in  free  space,  and  (4)  all 
C-functions  in  F/  satisfy  definition  (l)  along  7,.  If  f'\  =  /'’,•»  i-®-)  ‘s  constructed 
with  o.xactly  the  same  C-functions,  then  7,-  is  strongly  equivalent  to  "V,-. 

Note  that  by  definition,  each  V,-  is  restricted  to  where  the  conditions  of  definition 
(l)  hold,  i.e.,  to  some  region  N R[Fi)  C  7^  where  all  C-functions  in  F,  constructing 
Vi  arc  maximum,  applicable,  non-rodundant  C-functions  within  families.  Now,  in 
general,  7,-  is  an  unbounded  level  set  which  cannot  lie  in  free  space  everywhere. 
Thus  by  convention,  we  also  restrict  7,  to  the  region  of  interest  NR{F^)  where 
all  C-functions  in  F'-  satisfy  definition  (1).  That  is,  NR{F-)  —  {X  \  F^  C  Ax  }. 
Instead  of  writing  IiC\  N R{F-)  everywhere,  this  convention  is  assumed  throughout. 

In  is  an  interesting  question  whether,  for  every  Voronoi  chain  Vi, . . . ,  Vfc,  there 
exists  a  connected,  finite,  corresponding  equivalent  or  strongly  equivalent  chain  of 
intersection  manifolds  7i, . . . ,  7i;i  (where  k  is  not  necessarily  equal  to  k').  Theorem 

(I)  shows  that  for  all  Voronoi  manifolds  V,-,  there  exists  a  strongly  equivalent 
intersection  manifold  7^.  These  7,-  might  not  form  a  connected  chain.  Theorems 

(II)  and  (III)  show  that  each  disconnected  pair  of  intersection  manifolds  7,  and 

can  be  connected  by  an  infinite  sequence  of  “bridges."  Each  bridge  is  an 
intersection  manifold  erpiivalent  (but  not  strongly  equivalent)  to  V,.  We  then  argue 
that  since  there  exists  an  infinite  bridge  sequence,  therefore  there  must  also  exist 
a  finite  bridge  sequence.  I’inally,  (theorem  IV)  we  show  that  there  exists  an  entire 
intersection  chain 

7,U  •U4' 

which  is  homotopically  ccpiivalent  to  the  entire  V'oronoi  chain 


Theorem  I:  Let  V*  be  an  m-dimcnsional  Voronoi  manifold,  constructed  from  a 
set  of  applicable,  non-redundant  constraints  A,  which  satisfy  dcnnitioii  (I)  along 
Vjfc.  Then  if  there  exists  an  intersection  manifold  Ik  of  level  C-surfaces  for  the 
constraints  A,  and  if  the  constraints  A  satisfy  derinition  (1)  along  Ik,  then  Ti  and 
Ik  are  hoinotopically  equivalent. 

Proof:  We  will  show  that  two  intersection  manifolds  constructed  from  the  same 
C-functions  at  different  levels  are  hoinotopically  equivalent.  Next,  we  demonstrate 
that  the  Voronoi  manifold  is  essentially  a  special  case  of  intersection  manifold. 

Let  Mk  be  an  m-dimensional  Voronoi  manifold. 


%=^{X\h{X)=:--.  =/„(X)}, 


(n  =  6  —  m  +  1) 


where  the  /,  are  chosen  from  A  as  defined  above  (see  definition  (1)).  Note  that 
the  value  of  the  /,  may  vary  with  X  G  "Vjt.  Let  Ik  be  a  (m  —  l)-dimensional  (or 
O-dimensional,  if  m  =  0)  intersection  manifold  of  level  C-surfaccs  constructed  from 
the  same  functions  /,■: 

Jjk  -  {  X  I  /,(X)  =  U,f2iX)  =  f2,  .  .  .  ,  fniX)  =  }. 

The  region  of  interest  for  "Vfc  and  Ik  is  of  course  restricted  to  NR{{  /,  }),  where  the 
functions  { /, }  satisfy  definition  (l).  Ik  differs  from  "V/t  in  that  on  "Vjt  '-he  values 
(levels)  of  the  functions  /,  arc  equal,  whereas  on  Ik,  they  are  not.  Furthermore,  on 
"Vk  ^bc  value  varies,  whereas  on  Ik  the  values  arc  fixed. 

Ik  may  be  expressed  as 

^*=n/i'w- 


The  claim  is  that  Ik  is  homotopically  equiv.alcnt  to  "V*,  that  is,  that  if  g  :  Zi’*  / 
is  a  chart  for  J*.  and  g'  :  Zi'*  -»  7  is  a  chart  for  Vk,  then  there  exists  a  continuous 
homotopy  deformation  h  :  Zi’*  X  —*  J  between  g  and  g'  such  that 


h(Y,0)  =  g{Y) 
h(Y,l)  =  g\Y). 

As  usual,  /'  denotes  the  unit  interval  [0,  1].  For  a  review  of  elementary 
homotopy  theory  consult  appendix  II. 

The  charts  g  and  g'  exist,  since  J*.  and  ‘Vfc  are  manifolds.  (Assume  without  loss 
of  generality  that  only  one  chart  is  required).  A  level  C-manifold  fi\f)  (for  some 
level  1)  is  a  5-dimensional  manifold  and  hence  there  exists  a  chart  C,  :  ->  T  for 

/r*(0-  demonstrate  such  charts  in  the  proof  of  claim  (I.l),  below. 

Let  i-\)  be  any  achievable  value  for  the  functions  /,  along  the  Voronoi  manifold 
"V*:,  that  is,  any  ty  such  that  there  exists  some  X  6  satisfying  definition  (1)  for 
which 


=  /„{X)  =  h. 


Now,  (I  —  t)t\  is  a  linear  combination  of  the  levels  f;  and  (\  for  /;.  Since  each 
level  C-surface 


is  a  manifold,  each  has  a  chart  of  the  form  C,-,  above.  If  t  6  [0, 1],  these  are 
C-surfaces  for  /,•  with  level  I  G  [^Vi^i]i  their  charts  may  be  p.arameterized* 
by  t.  Suppose  we  have  a  set  of  several  level  C-surfaces  (as  in  J^).  Their  charts 
may  be  intersected  to  form  a  new  chart  for  the  intersection  manifold.  We  define 
h  :  X  1^  —*  7  to  he  the  chart  for  the  intersection  manifold  at  t,  such  that 


We  call  fi  a  chart  faintly  for  the  intersection  manifold. 

Claim  (l-l)'-  The  chart  family  h  can  be  constructed  such  that  h{Y ,  t)  is  continuous 
in  Y  and  t,  within  the  area  of  interest  for  Vfc  and  J*.  (For  proof,  see  below). 

*S<'('  itic  proof  of  rliiitii  (I.l),  wlioro  h(;  is  siiirli  a  rliarl. 
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Recall  that  ti  is  the  value  (or  level)  of  fi  on  the  intersection  manifold  J/t-  Then 
h  :  K*'  X  /*  — +  /  is  a  homotopy  between  g  and  g',  that  is,  h  continuously  deforms 
lie  into  Vit(^v)>  where  we  use  Ti{^-v)  ^  Vt  to  denote  the  Voronoi  manifold  restricted 
to  level  t'\i,  i.e., 

Vkitv)  =  { X  I  /,{X)  =  •  - •  =  UX)  =  h}cT. 

Verify  that 

h{E\l)  =  f)fr\t,) 

=  i 

and  that 

k;E‘,o)  =  n/.''(«v) 

=  V4(<v). 

We  have  shown  that  J*  Vjt(£v)  {—h  denotes  homotopic  equivalence)  for  all 
achievable  We  must  now  show  that  Vfc(€v)  —h  Tt(^-v  +  e). 

We  are  interested  in  continuous  deformation  within  7.  Hence  V*  may  be 
multiply  connected  within  7,  so  long  as  it  docs  not  wrap  around  obstacles.  Vit  must 
be  contractible  to  a  point  (within  /).  This  is  guaranteed  by  the  construction  of  A 
(definition  (l))  and  "Vfc,  i.e.,  by  the  choice  and  domains  of  the  functions  To  see 
this,  consider  that  if  "V*  did  wrap  around  an  obstacle  in  C-Space,  then  the  value  of 
some  fi  would  have  to  go  negative.  Hence,  it  would  become  redundant,  and  could 
not  be  used  in  the  construction  of  "Vi-  Note  that  the  C- Voronoi  diagram,  which  is 
the  union  of  Voronoi  manifolds  such  ivs  Vjt,  will,  in  general,  wrap  around  obstacles 
and  be  multiply  connected. 

Furthermore,  we  can  choose  c  such  that  the  topology  of  "Vt  does  not  change 
too  drastically  between  "Vt(^v)  and  Vt(^V  +  *)•  (This  is  possible  since  V*  is 
finite-branching).  So 


/*  —h  —h  f l)  — A  +  t-'z)  —/••••  ~h  "V|t(^v  +  fr)- 


Wliat  we  have  shown  is  that  Jjt  is  homotopically  equivalent  to  the  “easy”  parts 
of  “Vfc  (where  tl>c  level  of  the  Voronoi  manifold  is  constant).  We  next  showed  that 
because  the  topology  of  "Vt  is  simple,  we  can  paste  together  these  restricted  Voronoi 
manifolds.  | 

Proof  of  Claim  I.l:  The  existence  of  a  continuous  chart  family  for  the  intersection 
manifold  is  based  on  our  knowledge  that  the  manifolds  exist  at  certain  levels,  and 
from  our  ability  to  demonstrate  such  a  chart  for  the  intersection  manifold.  In 
chapter  4,  we  exhibited  C-functions  of  six  variables  for  the  C-Space  3?^  X  50(3); 

ft  :  -*•  » 

which  are  continuous,  affine  in  x,  y,  and  z,  and  multilinear  in  the  sines  and  cosines 
of  the  angles  %l),  0,  and  <t>.  The  Linear  Form  for  a  C-function  /,■  :  3?'*  X  50(3)  -*  3?, 
is  an  equivalent  expression 

fi{x,y,z,e)  =  7-:,  (0)1  +  E2(e)y  +  E.i{e)z  +  E^{e),  (6.i) 

where  Ej  :  50(3)  — ►  3?  (for  j  —  1, 2, 3, 4).  Now, 

fi  'it)  =  ker^/(i,  y,  z,  tj/,  0,  </>)  - 
A  chart  for  /,  *(^)  may  be  found  by  solving 

fi{x,y,z,ilj,0,^)-l  —  0 
for  X  (or  y,  or  z)  in  terms  of  the  other  variables  and  t: 


E^iV  +  £^32  +  —  I 

X  _  —  . 

(We  have  dropped  the  ©,  since  the  functions  Ej  arc  constant  with  respect  to  x,  y, 
z,  and  1).  If  El  =  0,  then  the  solution  for  y  or  2  may  be  employed.  This  yields  the 
obvious  chart 


C  :  E®  X  50(3) 

/  I  n  A\  /  +  ^3^  +  E^-l  ,  n  A\ 

[y,  2.  V>.  0, 4>)  >-*■  ( - - ,y,z,xl),0,  <i>) 

which  we  presented  in  chapter  4.  C  is  afTinc  in  I,  and  can  be  used  to  construct  a 
family  of  charts 


hc  :{Y,l)^^^  X  50(3) 


for  the  C-surfaces 


which  is  continuous  in  Y  =  {y,z,xp,0,<p)  and  t.  he  is  clearly  a  homotopy  between 
level  C-manifolds  for 

Chapter  4  also  derives  charts  for  fk\^k), 

(and  so  on)  by  solving  the  CTunctions  simultaneously  for  the  intersection  manifold. 
For  example,  a  chart  for  the  intersection  manifold  of  degree  three 

n  /.■'(«.) 

.-1,2.3 

can  be  constructed  by  solving  three  simultaneous  equations  with  the  form  of  (6.1). 
For  arbitrary  cocfIicient.s  Ej  and  levels  intersection  may  not  always  exist. 

However,  we  know  a  priori  that  it  exists  for  the  specified  levels  f,  and  From 
the  form  of  (6.1),  it  is  clear  that  if  the  intersection  manifold  exists  for  some  levels 


t{  (and  if  the  coclficicnt  functions  ICj  arc  independent),  then  it  will  exist  for  all 
levels.  We  onut  a  discussion  of  intersection  manifolds  of  higher  degree;  the  reader 
is  referred  to  chapter  4  for  further  details.  | 

Corollary:  For  every  Voronoi  manifold  "Vi,  there  exists  a  strongly  equivalent 
intersection  manifold  2,-. 

Corollary:  If  F,  is  the  set  of  constraints  used  to  construct  V,,  let  N  F(F,)  denote 
the  region  in  frce-space  where  all  of  the  constraints  in  F,  satisfy  definition  (1).  If 
"V,-  exists,  then  in  every  connected  component  of  NR{Fi),  there  exists  a  strongly 
equivalent  intersection  manifold  J,  built  out  of  F,-. 

Proof  of  corollaries:  All  /  G  F,-  exist  within  iV/?(F,).  Pick  any  X  E  NR{Fi). 
Evaluate  all  the  functions  in  F,-  at  X  to  obtain  a  set  of  levels.  The  intersection 
manifold  must  exist  at  these  levels,  since  we  have  demonstrated  that  X  is  on  the 
intersection  manifold.  The  intersection  manifold  from  the  C-surfaces  at  these  levels 
is  by  definition  and  by  theorem  (I)  strongly  equivalent  to  Vi.  | 

Next,  observe  that  for  all  1  <  i  <  k,  either  V,-  C  "K+i  or  ^i+i  C  In  other 
words,  to  move  from  T,'  to  X'+i>  we  either  add  or  remove  one  or  more  constraints: 

Ti-{X|/,(X)  =  --.  =/*,} 

V.+,  =  {A- 1/, (X)=--  =/,,„) 

and  cither  A:,-  >  ki^\  or  A:,-  <  A:,+|.  We  call  A;,-  and  A:,+  i  the  degree  of  the  Voronoi 
manifolds. 

We  have  shown  that  for  a  Voronoi  chain  V|, . . . ,  "V*,  a  sequence  of  intersection 
manifolds  may  be  constructed  such  that  each  7,  is  strongly  equivalent 

to  V,  (for  1  ^  i  <  A:).  However,  the  sequence  of  intersection  manifolds  may 
be  disconnected.  Wc  now  furnish  a  theorem  demonstrating  that  the  intersection 
manifolds  may  be  constructed  in  such  a  manner  that  they  ran  be  connected  together 
by  a  series  of  special  intersection  manifolds,  called  bridges. 


Theorem  II:  The  intersection  manifolds  I\,  I2, . . . ,  Ik  niay  be  constructed  such 
that  each  pair  of  intersection  manifolds  7,-  and  7,-.  |  can  be  connected  by  a  sequence 


of  “bridges.”  Each  bridge  is  an  intersection  manifold  equivalent  (but  not  strongly 
equivalent)  to  V,-. 

Proof:  Let  J\  be  a  strongly  equivalent  intersection  manifold  to  "Vi,  constructed 
with  C-functions  F'l-  Along  Ji,  all  constraints  in  Fj  are  non-redundant.  Let 
be  the  next  Voronoi  manifold  after  Vi  in  the  Voronoi  chain,  and  let  be  the 
constraints  constructing  "V^. 

Case  (i):  If  Fz  C.  F\,  then  "Vi  is  lower  in  dimension  that  V2.  We  can  construct 
Iz,  a  strongly  equivalent  intersection  manifold  to  V2,  which  is  connected  with  I\, 
be  removing  one  or  more  constraints  in  /'V  (We  remove  exactly  the  constraints 
F\  —  Fz).  This  is  possible  because  if  Fz  C  F\,  then  NR[F\)  C  N R{Fz): 


/leF, 


C\  /7'fe)- 


Strictly  speaking,  equation  (6.2)  should  employ  the  subset  notation  (C)  instead  of 
equality  (=),  since  I{  and  Iz  are  restricted  to  where  the  intersection  is  applicable  and 
non-redundant.  However,  the  equality  makes  the  construction  more  transparent. 
Note  that  the  construction  still  works  with  the  subset  notation,  since  N R[F\)  is  a 
subset  of  NR[Fz).  Since  Fz  C  F\,  I\  and  Iz  agree  on  the  levels  for  C-functions  in 
Fz-  Since  I\  C  /21  -^l  and  Iz  are  connected. 

Case  (ii-a):  Suppose,  however,  that  Fi  C  Fz-  Then  1^2  is  lower  in  dimension 
than  Ti,  and  NR{Fz)  C  NR{Fi). 

We  know  that  Ji  C  NR[Fi).  If  Ii  f)  H F{Fz)  ^  0,  then  we  can  construct  Iz 
from  Fz  such  that  Iz  C  IVR{Fz),  Iz  Q  I\,  and  in  addition,  I\  and  Iz  agree  on  the 
levels  in  F\.  Construct  !>  as  follows:  pick  a  point  Xo  C  7i  r\NR{Fz).  Evaluate  each 
fj  €  Fj  —  F|  at  Xo,  to  obtain  a  level  Cj  =  /.(Xo).  Construct: 


We  showed  earlier  that  I2  may  be  constructed  in  this  manner. 

Case  (ii-h):  The  hard  case  is  when  Jj  r\^R(F2)  =  ^his  case,  we  must 
construct  some  strongly  equivalent  (to  V^)  intersection  manifold  I2  C  NR{F2)  with 
different  levels  from  Ii  with  respect  to  the  C-functions  Fi.  We  then  build  a  sequence 
of  bridge  manifolds,  entirely  within  N R{Fi),  between  Jj  and  I2,  connecting  them 
together. 

The  bridge  intersection  manifolds  are  constructed  out  of  some  subset  Fy  C.  Fi, 
and  each  bridge  manifold  is  equivalent  (but  not  strongly  equivalent)  to  Vi-  The 
bridge  manifolds  are  formed  by  relaxing  one  or  more  constraints  in  F\  to  be  able 
to  move  from  N R{F\)  —  NR[F2)  into  NR[F2)-  The  motions  slide  along  intersection 
manifolds  constructed  from  the  remaining  constraints.  Once  inside  NR{F2),  we 
construct  J2  there,  and  I2  is  strongly  equivalent  to  ^2.  Note  the  levels  at  which  I2 
is  constructed,  (with  respect  to  the  constraints  in  Fj),  arc  typically  different  from 


KR(F,'^ 

Figure  6.4.  Case  where  /i  f|  N R{F2)  =  0. 

the  levels  at  which  Ji  is  constructed.  The  existence  of  bridge  manifolds  is  covered 
in  theorem  III.  | 

Wc  will  use  the  dot  notation  for  the  Riemannian  inner  product  on  the  tangent 
space  at  X.  The  normal  to  a  level  C-surface  is  V/,  and  depends  on  the 

inner  product.  We  assume  throughout  that  the  normals  are  unit  vectors.  A  basic 
concept  in  these  proofs  is  that  of  general  position,  or  transversality  (sec  Hirsch, 
1976).  Two  submanifolds  M ,  N  of  a  manifold  V  are  in  general  position  if  at  every 
point  of  Mr\N  the  tangent  spaces  of  M  and  N  span  that  of  V.  If  A  and  B  are 
not  in  general  position,  then  arbitrarily  small  perturbations  of  one  of  them  will  put 
them  in  general  position.  In  our  case,  M  and  N  correspond  to  level  C-surfaccs  and 
their  intersection  manifolds,  and  V  to  3?^  X  50(3).  The  proofs  still  work  even  if 
M  and  N  are  in  general  position  only  at  "many”  points  of  A/  fl  We  will  write 
the  condition  of  general  position  for  two  level  C-surfaces  9~*(^2)  as 


Theorem  111  The  Existence  of  Bridge  Manifolds:  (Bridges  of  dimension  five). 
Let  5  be  a  path-connected  component  of  NR{F\),  Xi),X]  €  S,  and  f,gEF[.  Note 
that  5  lies  with  in  the  domain  of  /  and  g,  and  that  by  construction,  i{S)  is  an 
open  set.  Then  if  V/  •  Vg  ^  1  within  S,  then  there  exists  an  infinite  sequence  of 
five  dimensional  bridge  manifolds  within  S,  connecting  ATq  and  X\. 

Proof:  Each  bridge  manifold  will  be  of  the  form  /~’(c/)  or  for  different 

levels  of  cj  and  Cg.  Note  that  as  we  vary  Cf,  f~^{cf)  covers  S  (similarly  for  g). 
Let  Tx  denote  the  six  dimensional  tangent  .space  at  X.  If  cq  =  f{X),  the  level 
C-surface  /“'(co)  is  a  five  dimensional  submanifold  of  3?'*  X  50(3),  with  a  five 
dimensional  tangent  space,  T^.  That  is,  identifying  Tx  with  a  subspace  of  Tx, 

=  |«.V/(X)  =  0}. 

It  is  easy  to  show  that  spans  Tx,  for  all  X  where  V f{X)  •  Vg[X)  7^  1. 

Let  N^x  denote  the  normal  space  at  X  with  respect  to  /,  such  that 

^i  =  {«€r.v|t;=--=aV/(X)}, 

for  all  scalars  a  €  ».  So  Tx  =  0  T^.  Clearly,  if  V/(X)  •  V!7(X)  ^  1,  then 

N!^  is  spanned  by  .  Hence  Tx  ~  Tx  + 

Since  the  space  of  differentially  tangent  directions  to  the  two  level  C-surfaces 
at  X  is  equal  to  the  space  of  all  directions,  there  exists  an  infinite  sequence  of 
differential  moves  along  level  C-surfaces  for  /  and  g,  at  different  levels,  to  realize 
any  path  within  5.  Since  5  is  connected,  there  exists  such  a  path  from  Xq  to  Xy. 

I 

Corollary  Ill.l:  (Bridges  of  dimension  four).  A  direct  result  is  the  existence 
of  a  sequence  of  bridges  which  are  four  dimensional  intersection  manifolds.  Let  f\, 
f'z,  h,  U  €  El.  Suppose  that  within  5,  V/,(X)  ■  V/y(X)  7^  1  (for  i  ^  j).  Then 
there  exists  an  infinite  sequence  of  bridges  between  Xn  and  Xi,  where  each  bridge 


w.i) = /.^'(»i)n/7'fe)- 


(•  ¥‘  i) 


This  is  reasonable,  since  each  Inihi)  is  a  four  dimensional  manifold.  If  the 
normals  are  all  independent  at  X,  then  the  direct  sum  of  the  tangent  spaces  to  all 
possible  intersection  manifolds  is  clearly  Tx'- 


Tx  =  (rj  +  (rj  nri’)  +  ■  ■•  +  (rj  nrj)  +  ■  •■  +  (rj 

(Of  course,  i  ^  j  for  all  terms  in  this  sum).  | 

Corollary  III. 2:  (Existence  of  a  finite  sequence  of  bridges).  We  now  argue  that  if 
there  exists  an  infinite  sequence  of  bridges  from  Xq  to  Xi  within  5,  then  there  also 
exists  a  finite  sequence. 


Informally,  wc  argue  that  it  is  always  possible  to  to  move  a  certain  distance  t 
along  each  level  C-surface,  and  that  this  e  cannot  grow  arbitrarily  small.  First  of 
all,  note  that  S  is  not  infinitesimal,  and  that  t(5)  is  an  open  set.  (If  it  were  not, 
it  might  be  necessary  to  make  an  infinite  number  of  differential  motions  to  remain 
within  5). 

We  also  appeal  to  the  well-behaved  structure  of  the  level  C-surfaces,  and  their 
intersection  manifolds.  The  level  C-surfaces  are  smooth,  with  normals  that  change 
continuously.  (If  the  normals  changed  discontinuously,  we  might  not  be  able  to 
take  finite  steps).  Thus  we  can  move  a  finite  (i.e.,  not  inCnitessimal)  distance  along 
the  surfaces  to  a  point  where  the  normals  are  still  independent,  and  where  the 
surfaces  are  “similar”  (i.e.,  having  normals  in  almost  the  same  direction  as  before). 
Furthermore,  for  any  two  levels  of  /  within  S,  there  exists  a  homotopy  between 
them.  These  continuity  arguments  indicate  that  it  should  be  possible  to  move  in 
finite  steps  along  the  intersection  manifolds,  and  hence  we  can  reach  Xj  from  Xq 
in  a  finite  number  of  bridges. 

Suppose  from  Xo  to  Xi  there  exists  an  infinite  sequence  of  bridge  manifolds, 
but  no  finite  sequence.  Then  either  (l)  i{S)  is  not  an  open  set  (and  therefore 
only  differential  motions  can  stay  within  it),  or  (2)  for  a  subset  PCS,  whose 
cardinality  is  that  of  the  continuum,  the  entire  tangent  .s])ace  is  not  available  along 
the  level  C-surfaccs.  In  both  cases,  our  initial  hypotheses  arc  violated.  (1)  violates 
the  assumption  i{S)  is  an  open  set,  and  (2)  the  assumption  of  general  position. 

We  formalize  this  argument  as  follows: 

Definition:  Let  li  he  a  metric  space,  and  p,  p'  :  f  U  be  paths  in  U-  Let  {  Pn  } 
be  an  open  cover  of  p(/')  in  It,  where  each  U„  is  a  neighborhood  of  radius  <  r, 
and  f/anp(-f')  7^  0-  We  say  that  p'  approximates  p  at  resolution  r  if  {  {/„  }  is  an 
open  cover  for  p^(f')  also;  that  is,  if  p'(/')  C  Uc, 

Claim  (III. 2.1)  shows  that  an  arbitrary  curve  in  some  neighborhood  U  of 
free-space  can  be  approximated  by  a  path  within  U  along  a  finite  .sequence  of 
intersection  manifolds.  The  proof  of  Cor.  (111.2)  then  employs  the  fact  that  the  curve 
is  compact,  and  therefore  can  be  covered  by  a  finite  number  of  such  neighborhoods. 
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P'iRure  6.6.  Let  cq  =  /(Xq)  and  Ci  =  f{Xi).  Xo  lies  on  /“‘('^o)n!/~’{<^o^)>  -^i  1'®*  o" 

n  "Tl'c  path  segment  between  Xq  and  X|  may  be  approximated  by  a  path  sliding 

first  along  g~^{c^  from  Xq  to  X',  and  then  from  X'  to  Xi  along  /~*(ci). 

Claim  III. 2.1:  Any  path  within  a  neighborhood  where  /  and  g  are  defined,  may 
be  approximated  to  an  arbitrary  resolution  by  a  finite  sequence  of  motions  along 
level  C-surfaces  of  /  and  g. 

Proof'.  We  will  regard  level  C-surfaces  of  /  and  g  as  trivial  intersection  manifolds. 
Consider  how  one  can  approximate  a  path  from  Xq  to  Xi  by  a  path  along 
intersection  manifolds.  Let  d  be  a  metric  on  3?^  X  5'(9(3),  and  /,  g  be  C-functions 
with  inverse  images  covering  a  neighborhood  containing  Ao  and  Xi.  Let  cq  =  /(Xq) 
and  Cl  =  /(Xi)*Xq  lies  on  the  intersection  manifold  /~'(co)n9  '(*^0)1  and  Xi  lies 
on  /~'(ci)  n  ' (cj).  Construct  /“'(ci)  H  (refer  to  fig.  6.G),  and  choose  X' 

to  be  the  any  of  the  closest  points  to  Xo  on  this  manifold.  We  can  construct  a  path 
which  slides  from  Xq  to  X'  along  ?~'(cn),  and  then  slides  from  X'  to  Xi  along 
/~*(ci).  We  wish  to  demonstrate  that  by  choosing  Xi  sufficiently  close  to  Xq,  X' 
can  be  made  to  lie  arbitrarily  close  to  Xq;  that  is,  for  all  £  >  0,  there  exists  a 

*  and  =  gfXp),  c  '  =  g(Xj). 
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5  >  0  so  that  d{XQ,X')  <  e  whenever  d{Xo,Xi)  <  6.  This  is  definitional,  since 
limATi-Xo  /"‘(ci)  =  /“‘(co). 

Thus  for  every  neighborhood  B,.(Xo)  of  radius  r  about  Xq,  there  exists  an  Xi  6 
Br{Xo)  such  that  X'  £  Br(Xo)  also.  Furthermore,  d(Xo,X')  is  finite  (and  non- zero). 
Of  course,  a  similar  argument  holds  for  the  path  segment  between  Xi  and  X'.  Thus 
any  path  within  a  neighborhood  where  /  and  g  are  defined,  may  be  approximated  to 
an  arbitrary  resolution  by  a  finite  sequence  of  motions  along  intersection  manifolds. 
I 

Clearly,  similar  arguments  hold  for  intersection  manifolds  of  higher  degree. 

Proof  (Corollary  III. 2):  (Sketch)  Let  p(/^)  be  a  path  within  i[S)  from  Xq  to  Xi 
along  an  infinite  sequence  of  intersection  manifolds.  In  short,  p{I^)  C  *(*5).  Choose 
an  open  cover  {  Ua  },  relative  to  3?^  X  50(3),  for  p{I^)  such  that  Ua  C  t(S), 
p{I^)  is  compact,  hence  there  exists  a  finite  subcover,  i.e.,  for  finitely  many  indices 
0(1, . . .,  Qt„,  we  have 

PU')CC/a,U---UC^a«Ct(5). 

Now,  for  each  we  can  construct  a  path  p\I^)  along  a  finite  sequence  of 
intersection  manifolds  approximating  p(/^)nf/o,  (Claim  III.2.1).  That  is,  p'{I^)  is 
also  contained  within  the  closure  of  f/,,..  Furthermore,  it  is  not  hard  to  construct 
p'  such  that  it  leaves  the  neighborhood  (/q,.  at  the  same  points  as  p,  that  is,  so 
that  p{I^)r\dUai  ==  p'{I^)C\dUcn.  Since  this  is  true  for  all  £/a,»  then  p(/*)  can  be 
approximated  by  some  path  contained  within  t^ai  U  *  •  •  U  and  which  lies  on 
some  finite  chain  of  intersection  manifolds.  | 

Theorem  (IV)  is  almost  immediate,  and  its  proof  similar  to  that  of  Theorem 

(I). 

Theorem  IV:  For  every  connected  chain  of  Voronoi  manifolds  Cy  =  •  •  • » 

there  exists  an  equivalent  (in  the  sense  of  theorems  (I — III))  connected  chain  of 
intersection  manifolds  Cj  —  such  that  the  entire  Voronoi  chain  Cy  is 

homotopically  equivalent  to  the  intersection  chain  Cj.  That  is, 


U  U 

v<eCv  /,eC/ 


Proof:  Simply  apply  the  fact  that  each  equivalent  pair  (V,J)  (where  V  is  a 
Voronoi  manifold  and  J  is  an  equivalent  intersection  manifold)  must  lie  in  the  same 
non- redundancy  region.  Hence  if  one  chain  wraps  around  an  obstacle,  so  must  the 
other;  furthermore,  each  chain  must  wrap  around  the  same  obstacles.  | 

Future  Research 

There  are  several  interesting  problems  which  are  left  open.  They  include  the 
following; 

(i)  We  have  demonstrated  an  equivalent  chain  of  intersection  manifolds  for  any 
connected  chain  of  Voronoi  manifolds.  Show  whether  or  not  a  strongly  equivalent 
chain  exists  also. 

(ii)  Show  whether  or  not  for  every  Voronoi  chain  Cv  there  exists  a  (strongly) 
equivalent  intersection  chain  exhibiting  a  bijoctivc  correspondence  to  C\. 

(iii)  Devise  an  effective  procedure  for  constructing  a  chain  of  intersection  manifolds 
to  realize  any  class  of  paths  in  free  space. 

(iv)  Derive  complexity  bounds  on  the  construction  of  the  C- Voronoi  diagram  and 
the  equivalent  intersection  chains. 

(v)  Other  configuration  spaces,  such  as  those  arising  in  the  hinged  body  problem, 
should  bo  considered. 

(vi)  To  extend  these  results  to  configuration  spaces  generated  by  real  space 
constraints  which  arc  not  polyhedral  (for  example,  algebraic  surfaces),  a 
generalization  of  the  (jVD  such  as  smoothed  local  symmetries  (Hiady,  1982b) 
could  be  considered. 
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Conclusion 

In  chapters  1  through  6,  we  developed  representations  and  algorithms  for 
automated  spatial  planning  with  six  degrees  of  freedom.  To  demonstrate  the 
competence  of  the  representation  and  the  feasibility  of  the  algorithms,  a  planning 
system  for  the  classical  find-path  problem  with  six  degrees  of  freedom  was 
implemented.  The  planner  is  of  considerable  intrinsic  interest,  in  that  it  is  complete 
(for  a  given  resolution).  Experiments  have  demonstrated  that  this  algorithm  can 
solve  find-path  problems  requiring  six  degree  of  freedom  solutions  that  were  beyond 
the  competence  of  earlier,  approximate  planners.  The  mathematical  framework 
developed  here  impacts  a  class  of  geometric  planning  problems  for  three  dimensional 
objects. 

The  planning  algorithm  may  be  explained  by  analogy  with  the  Point  Navigation 
Operators.  'Phe  C-Space  transformation  reduced  the  motion  planning  problem  to 
the  task  of  navigating  a  point  in  9?’  X  50(3).  Since  the  path  for  the  point  must 
avoid  the  C-Space  obstacles,  which  are  curved,  six  dimensional  manifolds  with 
boundary,  clearly  paths  can  be  found  in  C-Space  by  the  closure  of  three  operators: 

(i)  slid  cs  along  1-  to  '1-dimensional  intersections  of  level  C-surfaccs; 

(ii)  slides  along  5- dimensional  level  C-surfaces; 

(iii)  jumps  between  G-dimonsional  obstacles. 


However,  these  simple  operators  could  not  be  implemented  until  a  series 
of  representational  and  algorithmic  questions  were  solved.  The  fundamental 
representational  issues  centered  on  the  structural  properties  of  the  defining  C- 
functions.  By  deriving  their  domains,  and  by  proving  theorems  about  the  topology 
of  these  domains,  it  was  then  possible  to  address  tiie  intersection  problem  for 
high-dimensional  configuration  spaces.  By  solving  these  open  problems,  developing 
representations  for  the  C-functions  and  their  domains,  and  designing  decomposition 
algorithms  in  C-Space,  it  became  possible  to  represent  the  constraints  on  motion 
“completely,”  and  to  exploit  the  complete  representation  in  implementing  a  planning 
algorithm.  Next,  we  presented  new  theoretical  results  on  the  C-Voronoi  diagram. 
By  showing  that  for  every  connected  chain  of  Voronoi  manifolds,  there  exists 
an  equivalent  chain  of  intersection  manifolds  of  level  C-surfaces,  a  theoretical 
completeness  result  for  planning  along  the  intersection  manifolds  was  obtained. 
'Phis  result  is  also  of  interest  since  while  the  charts  for  the  Voronoi  manifolds  are  as 
yet  unknown,  charts  for  the  intersection  manifolds  are  straightforward  (given  our 
representational  framework).  Thus  it  is  possible,  in  principle,  to  devise  a  planning 
algorithm  with  all  the  advantages  of  a  planner  along  the  CVD. 

There  is  much  work  to  be  done.  Ultimately,  decomposition  algorithms  such  as 
those  we  present  in  chapter  5  will  become  increasingly  important  in  partitioning 
C-Space  into  regions  where  the  set  of  applicable  (or  alternatively,  relevant) 
constraints  is  invariant  (see  also  Schwartz  and  Sharir  (1981)).  The  represent.alions 
and  algorithms  vve  have  developed  may  make  other  geometric  planning  problems-- 
such  as  fine-motion,  and  planning  with  uncertainty — more  feasible,  and  should  now 
be  applied  in  these  applications.  The  find-path  algorithm  can  be  easily  extended 
to  robot  manipulators  with  six  degrees  of  freedom  in  which  translations  can  be 
decoupled  from  rotations.  This  class  includes  Cartesian  manipulators  (for  example, 
the  IBM  RS/J).  The  adaptation  of  this  work  to  a  production  environment  presents 
interesting  engineering  challenges. 

In  principle,  the  (>DOF  rcj)rese.ntations  can  be  extended  to  revolute-joint,  linked 
arms  with  six  degrees  of  freedom.  However,  the  C-Space  of  the  linked-arm  problem 
is  the  six  dimensional  torus. 
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which  has  a  very  different  structure  from  9?^  X  SO(3).  It  is  our  hope  that  this  thesis 
can  present  a  methodology  for  formulating  the  geometric  constraints  for  arbitrary 
configuration  spaces,  and  that  a  similar  structure  will  be  found  for  constraints  on 
the  6- torus. 

Our  planning  algorithm  is  complete  (at  a  resolution),  in  that  the  representation 
employed  is  complete,  and  in  that  the  search  is  guaranteed  to  find  a  path  if  one 
exists  at  that  resolution.  However,  since  it  is  a  search  algorithm,  we  cannot  provide  a 
polynomial  time  bound.  Our  motivation  has  been  to  address  the  completeness  issue 
Erst,  by  resolving  fundamental  representational  questions;  now,  one  of  the  most 
important  remaining  tasks  is  to  develop  complete,  polynomial- time  algorithms  which 
can  actually  be  implemented.  We  do  not  believe  that  the  worst-case  exponential 
behavior  of  the  6DOF  planner  is  inherent  in  the  representation,  and  conjecture 
that  a  polynomial- time  algorithm  which  plans  paths  along  intersection  manifolds 
can  be  devised.  Indeed,  the  theoretical  results  on  the  CVD  are  suggestive  that  the 
limiting  complexity  of  the  approach  may  be  the  complexity  of  constructing  the 
CVD  or  an  equivalent  chain  of  intersection  manifolds.  More  research  is  needed  on 
the  topology  of  the  CVD.  A  fast  planning  system  might  determine  what  constraints 
construct  the  CVD,  and,  using  these  constraints,  construct  a  chain  of  intersection 
manifolds  which  could  attain  the  goal.  The  first  step  in  this  effort  would  bound  the 
complexity  of  the  CVD  and  the  intersection  chains. 
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Appendix  I 

Details  of  the  Intersection  Problem,  and  Related  Problems 

This  appendix  contains  the  detailed  equations  from  chapter  4,  which  are 
relegated  to  an  appendix  because  of  their  excessive  length.  Some  definition  of  terms 
is  repeated,  so  that  the  interested  reader  will  not  have  to  flip  back  and  forth  too 
much. 

I.l.  Intersecting  Two  C-surfaccs  in  3?^  X  5’ 

By  applying  trigonometric  reductions  we  can  express  type  (a)  and  (b)  constraints 
as  follows  (only  (al)  and  (bl)  are  shown): 

cos(X,)5y  +  Csin(X,)y  —  sin(X,)5'i  +  (7  cos(X,)i 
+  sin(X,'  -  7y)||<»y||S’  -  lia.i|  cos(X,-  -  77,) 

-  (7cos(Xi  -  7j)j(6y((  (al) 

sin(^y)2/  +  cos(<^j)x  -  ||a,l!  sin(^y  -  77, )5 

-  C||a,||  cos(</>j  -  77,)  -  ||6y)|  co.s((6y  -  7y)  (hi) 

Where 

C  =  cos  0  and  S  =  sin  0. 

Now,  we  can  consider  a  pair  of  these  equations  as  a  system  in  four  variables, 
[x,y,C ,S),  and  proceed  to  solve  (al)  and  (b2),  (bl)  and  (b2),  and  (al)  and  (bl)  for 
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X  and  y.  For  example,  the  intersection  of  two  type  (a)  svirfaccs,  (al)  and  (a2)  is  a 
curve 

P  ■  r„\  f)ra2  -»  K'**  X  S‘ 

where  r„|  fl^aa  C  -5’  denotes  the  intersected  applicability  constraints  for  (al)  and 
(a2).  Although  the  solutions  arc  in  the  variables  C  and  S,  we  can  use  C  —  cosO  = 
cosr  and  S  =  sinO  —  sinr  to  generate  the  curve  of  intersection  in  X  S'*. 

After  much  simplification,  the  parametric  solutions  for  the  intersection  curves 
for  type  (a)  and  (b)  constraints  in  9?^  X  5'  are  as  follows: 

The  intersection  of  two  type  (a)  Surfaces:  {al)n(«2) 

Pair)  -■=  r, 

Pi(r)  =  Z;^-(S’(||uil|  cos{ri-  4-  X,  -  X')  +  lja,'||  cos(r?/  -  X,-  -  X,-) 

-  ||a,||  cos(X'  +  r)i  ~  X.)  -  ||ai||  cos(-X,'  +  rji  -  X,) 

+  C(2cos(X?  +  Xi  -  ^')116'1|  -  2cos(X'  +  X^  -  ij)\\bj\\)) 

+  ^(lla.'ll  sin(r7(  +  X,-  -  X/)  -  ||a'j|  sin(r?/  -  X,-  -  X,0 

-  ||a,||  sin(X  •  +  r?.-  ~  X.)  +  ||a.||  sin(-X/  +  77, ■  -  Xj)) 

•f  C\2  sin(X ?  +  X.-  -  7;)1|6;|1  -  2  sin(X;  +  X,  -  7>)|16;11) 

+  (-  sin(X/  +  Xi  -  Tfy)  -  sin(X/  -  X,-  -  7j))l|t>ll 

+  (sin(X.'  +  \i  -  Tfy)  -  sin(X/  -  X,  +  1f>))|IMI)/2), 

Py{r)  =  /;^-(5(||a;||  sin(77/  +  X,-  -  X/)  -  l|n/||  sin(r7;  -  X,-  -  X/) 

-  ||a,||  sin(X  ■  +  77,  -  X,)  +  ||a,||  sin(-X,-  +  77,-  -  X,) 

+  C7(2sin(X'  +  Xi  -  7')1|6'||  -  2sin(X'  +  X,  -  7;)||t,||)) 

+  C'(-||a-||  cos(77/  +  X,-  -  X/)  -  ||a/j|  cos{rj'i  -  X,-  -  X/) 

+  llavll  cos(X-  +  T]i  -  Xi)  4-  ||n,|l  cos(-X-  4-  m  -  X,)) 

4-  C'2(2cos(X;  4-  X.-  -  7;)|16>1|  -  2cos{\[  +  X,-  -  7;)||6;.||) 

+  (cos(X/  4-  X,-  -  7})  -  cos{Xf  -  X,-  -  7j))ll^y|| 

4-  (cos(X/  -  X,-  -f  7y)  -  cos(X/  4-  X,-  -  7>))ll^il)/2), 


where 


The  IritcrsccLion  of  type  (a)  and  (b)  Surfaces:  (al)n{^2) 


Po{r) 

PAr) 


=  r. 


-f 

+ 

+ 

+ 


F(5{2C'||a'||cos(^;-Tj/~X,) 

-  Ij)  -  lltjll  cos{4>'j  +  X,-  -  7y) 

-  -  Ij)  +  ll^jll  ~  +  7jj) 

-  v!  +  >^i)  -  2C’-||a'||  sin(<^j  -  tj-  -  X, 

-  Pi  -  h)  -  ll«.il  sin('^j  +  Vi  -  >^i) 

-  llu.lhi'K'Aj  -  Vi  +  K) 

+  sin(<^j-  +  \i-  7j)  -  ll<»ill  sin((^j-  +  X.-  -  7y) 

-  ||6j||  sin(</)j  -  \i  -  7j)  -  ||6yj|  sin{<Aj  -  X.-  +  7;))j, 


cos(<Ay 

cus(^j 

sin(</>j 

sin(<Aj 


Py(^)  =  -'^'’(‘S'(26’||a'|l  sin(<^j-  -  tj-  -  X,) 

-  Ijbyll  sin(</)j  +  Xi  -  7])  +  lltyll  s\n{(l>'j  +  X,-  -  7y) 

+  Ill'll  sin(0y  -  K  -  Ij)  -  IIMI  sin(<?iy  -  X,-  +  7y)) 

+  Ha'll  cos{<6'  -  nl  +  X.)  +  2C'‘'||a;||  cos(^;  -  rj-  -  \ 

-  ||a'||  cos{4>'j  -  ril  -  X.)  -  Ija.ll  cos((?i}  +  r/,-  -  X,) 

-  ||a,l|  cos((/>y  -  T)i  +  X,-) 

+  C(l|ij||  c.os{<j)'j-  +  \i  -  7y)  -  I16y||  cos{<i>'j  +  \i  -  7y) 
+  ||6;||  cos((Ay  -  X,-  -  7y)  -  ||6y|l  cos{^6j  -  X,'  +  7y))j, 


(2  cos(0y  —  \i)S  —  2C  sin(^y  —  X,)) 
csc(0  —  </>'■  4  Xj) 


where 


The  Intersection  of  two  type  (b)  Surfaces:  (61)  0(^2) 


Vo{r)  =  r, 

Px{r)  =  E^(5(||a'||  c.os{<i)'j  +  6j  -  »//)  -  ||a,||  cos(<Ay  +  4>j  -  Vi) 

-  lla.'ll  -  <t>j  -  v'i)  +  cos(<^j  -  (j>j  +  Vi)) 

+  C'(-||a,'||  s\n{<t>j  +  <f>j  -  Vi)  +  ll«.i|  sin((^y  +  (/>y  -  Vi) 
+  sin(</>y  -  <t>j  -  v')  +  ||a.!|  sin(<^j  -  (t>j  +  Vi)) 

-  Il^yll  s»n(<Aj  +  -  If})  +  ll^jll  sin((f>}  +  -  7,) 

+  ||6;1|  sin(<^j  -  <f>j  -  v})  +  \\bj\\ sin{<^j  -  <f>j  +  7y))/2  j, 

P»(^)  =  '^((■S'dk.'ll  +  <Aj  -  v!)  -  llflill  +  <f>j  -  Vi) 

+  ll“.■|l  -  ’n'i)  +  lla.II  sin{(j>}  -  (t>j  +  Vi)) 

+  Cdladl  cos{4>}  +  <f>j  -  Vi)  -  ||a.||  cos(<f>}  +  (t>i  -  Vi) 

+  list'll  <^°^(<t>}  -  <f>j  -  Vi)  -  ||a.||  cos(<;6j  -  <6y  +  Vi)) 


+  ||6yll  COs{4>'-  -f-  4>j  -  -i'j)  -  I16yl|  COs(<^j  +  0y  -  7y) 

+  co«(^y  -  4>j  -  7y)  -  ll^yll  cos(^'-  -  <j>j  +  Vj))/2^ 


E  =  csc(^'-  - 
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1.2.  Related  Problems  in  3?^  X  5* 

1.2.1.  Techniques  for  Moving  Along  C-Surfaces  in  9i“  X  S’ 

In  this  section  we  present  techniques  for  moving  along  a  C-Surfacc.  We  could 
imagine  using  these  methods  to  move  to  the  nearest  “edge”  (C-Surfacc  intersection), 
for  example.  A  level  C-Surface  is  defined  via  a  function  f[x,  y,0)  —  k  for  k  constant. 
/  is  exactly  of  form  (al)  or  (61)  (above),  and  the  level  surface  in  3?’’  X  s'  is  all 
points 

L={Xe^'^Xrf\f{X)  =  k}, 
where  rj  C  S'  is  the  0  applicability  range  for  /. 

Define  a  hyperplane  in  3?^  X  S'  as  the  set 

P  =  {  X  €  3?-  X  S'  I  X  •  //  =  --64  }, 
where  II  —  [h\,  h-i,  hi). 

We  intersect  the  level  surface  L  with  the  hyperplanc  P  to  obtain  the  intersection 


'  I 

G 1  ^S(cos(X,)(-h3f  -  hi)  +  h-i  sin(X,-  -  7y)||t>H] 
+  C'sin(X,)(— hyt  —  hi)  —  h2||a,||  cos(X,  —  77,) 

-  Ch2  cos(X,-  -  7y)||6y||  -  h-ik^ 

-G|^S(sin(X,)(h;,l  +  hi)  +  h,  sin(X,  -  7y)||<»y||) 
+  C'cos(X,)(-/i3t  -  hi)  -  /ii||o,||  cos(X,-  -  rji) 

-  Chi  cos(X,-  -  7_,)|(6y||  -  hik^, 


iPn(al)) 


where 


G,  = 


(h'j  sin(X,)  +  hi  cos(X,))S  -}  (hi  sin(X,)  -  h^  cos(X,))C 


'•  .*•  . 


^ 


where 


Po{t)  =  r, 

Px{t)  =  -G>(su\{4>j){h:it  +  h^)  +  /i2||a,||sin((;6j  -  t;,)5 

+  C'/i2||a,||cos(<^y  -r?,) 

+  h2||6J|cos(0j  -  7y)  -I-  hik^ 

Py{t)  =  G>{cos{(j>j]{h:^L  +  hi)  +  hi |[a,j|  sin(<^y  -  77, jS 
+  C'hi|la,||cos(0j  -  r]i) 

+  h|||/)y||cos(<^j  -  7j)  +  hifcj, 


hi  sin(<^y)  —  ho  cos(<^y) 


{^n(M)) 


1.2.2.  Characterizing  Clearance  to  a  C-Surface 

It  would  be  very  useful  to  characterize  the  minimum  clearanct;  to  a  C-suiface. 
We  would  like  to  answer  the  question: 

•  For  a  point  h^y  €  3?“)  o.t  what  orientation  is  bry  closest  to  a  C-surface,  and 
what  is  minimum  directed  clearance  vector  at  that  orientation? 

Using  Lagrange  multipliers,  we  can  minimize  a  function  f{x,y,0)  subject  to  a 
constraint  g{x,y,0)  =  0  by  constructing  the  auxiliary  function 

H[x,y,0,t)  =  f{x,y,0)~ig{x,y,0) 

and  solving  the  partial  derivatives 

If-  <■) 

dH 

Do—o  P) 

dll  ^ 

-^=0.  (4) 

In  our  case,  g  will  define  a  C-surface,  for  example,  a  type  (a)  surface: 
g{x,y,0)  =  sin(0  +  \i)y  +  cos(<l  -t-  X,)i  -  |j6_,||cos(0  +  X,  -  7^)  -  1|h,1|cos(X,  -  rj,) 
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and  /  will  be  a  distance  function.  Now,  the  rotational  dimensions  cannot  be  treated 
uniformly  in  establishing  a  metric,  so  we  will  define  distance  in  Iviclidean  space. 
Minimizing  the  square  of  the  translational  distance  sulliccs  for  our  purposes.  Hence, 


f{x,  y,  0)  =  {x-  bxf  +(y  -  byf. 
Differentiating  11  gives  us  the  following  equations; 


dll 

dx 

dH 

dy 

dll 

dll 

dl 


=  2[x  —  6j.)  —  t  cos{0  +  \i) 

=  2{y  —  by)  —  t  sin((?  +  X^) 

=  — ^(cos(<?  +  X,)y  —  sin(<?  +  X,)i  +  \\bj\\  s\n{0  +  X,  —  7;)) 

=  (—  sin(0  +  \i)y  —  cos(0  +  X,).t  +  j)6y||  cos(0  -h  X,-  —  jjj 
+  ||a,  ||cos(r7,-  -  X,)) 


(1) 

(2) 

(3) 

(4) 


Solving  these  equations  for  x,  y,  0,  and  I  is  not  trivial.  However,  the  following 
observations  make  it  easier.  First  of  all,  we  note  that  solving 


i 


dH 

dl 


=  0 


(5) 


is  equivalent  to  solving  (4)  as  long  as  ^  =  0  is  not  a  solution.  We  next  solve  (1)  and 
(2)  for  lc.os{0 -{■  X,)  and  ^sin(<7-f-  X,)  and  substititute  this  value  in  to  (3)  and  (5). 
(5)  then  becomes  a  linear  equation  in  x  and  y  while  (3)  is  quadratic  in  x  and  y  and 
linear  in  t.  Our  rewriting  of  (4)  into  (5)  has  thus  eliminated  I  from  (5),  and  we  can 
solve  for  i  in  terms  of  y. 


-2i/  t  (2||fcyH  sin(7y)  -I-  2by)y  -  2x^  +  (2||6_,1|  cos(7y)  +  26x)i 

+  ‘■os(r/,  -  X.)  -  2by\\bj\\ sin(7y)  -  26^||6^||  cos(7y)  (3) 

{2b,  -  2||f)_,||  cos(7;))y  +  (2||6y||  sin(7j)  -  26„)z 

-  2/;, !!/;_, II  sin(7j  f  26j,l|6y||  cos(7y)  (5) 

Wo  need  one  additional  constraint;  this  is  obtained  by  observing  that 


sin(^  +  X,)**  -I-  cos{0  -1-  X,)'^  —  1.  (6) 

Since  the  trigonometric  terms  can  be  expressed  in  i,  y,  and  f,  we  can  obtain  l^  in 
terms  of  .r.  and  y.  (3),  (fi),  and  (b)  then  result  in  a  quartic  in  x  with  the  roots: 


where 


Ci|^26„|16_,|p  sin(2'r;)  +  26:,l|6_,lp  008(27^) 

-  ((26^-  +  26‘i)||6,l|  +  2||/,,||'^)  cos(7y)  +  26,||6,||'^ 

±  ^2(^110111116^1  cos(r?,-  -  X,-  4-  'Ij) 

+  ||a/HI|6y||  c.os{T]i  -  X,-  -  7y)  -  26^||a,ll  cos(77.-  -  X,- 


G,  = 


2^26yl|6y||sin(7y)  +  26^||6y[|  cos(7y)  -  HfeyH'-*  -  bl  - 


G2  =  v^-26„||6j||  sin(7_,)  -  26^116y||  cos(7y)  +  ||6y||'-^  +  6^  +  62 
Given  x,  y  is  found  from  (5)  : 

(||6yi|  sin(7y)  -  by)x  -  i^jjuyjj  sin(7y)  +  6j,||6y||  cos(7y) 


||6y||cos(7y)  -6* 

I  can  bo  found  from  (3)  as  a  linear  function  of  x  and  y.  To  determine  0,  we  calculate 
sin{0  +  X,)  and  cos(0  +  X,)  from 

sm(iH- X.)  ~ 

cos(9  +  Xi)  = 

and  use  a  two  argument  arctangent  function  Atan2  :  3?'^  5'  to  determine  0  +  X,-. 

The  0  value  must  be  checked  against  the  applicability  constraints  for  surface  y, 
if  it  falls  outside  the  range,  then  endpoints  will  yield  the  minimum  clearance. 
Naturally,  it  is  possible  that  for  certain  orientations,  {bj.,by,0)  will  lie  on  or  inside 
the  C-surfacc.  These  cases  may  be  disambiguated  by  the  sign  of  g{b/,by,0).  Finally, 
given  the  closest  point  (at  some  orientation  0)  on  the  C-Surface,  the  minimum 
clearance  is  simply  the  vector 

{^,y)  -  {bz,by). 


The  Minirmirn  Clearance  to  a  type  (b)  C-surface 

To  find  the  ininimurn  clearance  to  a  type  (b)  surface  and  the  orientation  at 
which  the  clearance  occvirs,  we  let  g  be  a  type  (b)  constraint  (eipvation  (bl))  and 
solve  the  system  of  partial  derivatives  of  H . 


=  2{x  ~bx)-  I cos{(j>j)  ( 1} 

~  =  2(y  -  by)  -  I s\n{(l>j)  (2) 

~  =  -^lluill  sin(0  -  4>j  -H  Tli)  (3) 

~  = -g(x,y,0)  (4) 

The  solution  is  considerably  easier  because  the  form  of  the  con.sl.raint  surface  is 
less  complicated.  Since  dH fdf.  =  — g,  0  may  be  found  in  terms  of  x  and  y  using  an 
arccosine.  Substituting  this  value  of  0  into  (3)  yields  a  quadratic  equation  in  x,  y, 
and  i,  which  when  solved  with  (l)  and  (2)  for  the  following  roots: 

I  =  ||6y||  cos(<?!.y)  cos{4>j  -  'Ij) 

—  by  c.os{<f>j)  sin(</)y)  —  6^  cos(^y)‘'^  —  ||ai||  c.os{(f>j)  +  b^, 

y  =  Ill'll  s\n{<j)j)cos{4)j  -  7y) 

-  {bx  cos{(l)j)  ±  ||a,|l)  s\n{(f>j)  +  by  cos(<Ay)^, 

I  ~  2llh_,|l  cos{4)j  -  7y)  —  2i>y  sin((^y)  —  2bx  cos{<l>j)  —  2|la,j|. 
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The  Connectivity  of  Configuration  Space 

n.l.  A  Review  of  Elementary  Ilomotopy  Theory 

In  this  appendix  we  review  some  elementary  homotopy  theory,  and  address 
the  connectivity  of  configuration  space.  See  Hocking  and  Young  (1961)  for  a  more 
extensive  review,  and  Donald  (1983a)  for  an  analysis  of  the  relation  between 
channels  and  homotopic  equivalence  classes.  Let  /’  denote  the  unit  interval.  A 
parameterized  family  of  mappings  from  a  space  X  into  a  space  Y  is  a  continuous 
function  /i  ;  X  X  1*  -+  Y.  Consider  the  mappings  /  and  g  from  X  to  Y :  we  say 
that  /i  is  a  homotopy  between  /  and  g  if  for  each  point  i  in  X, 

h(x,0)  =  /(x)  and  h(x,  l)  =  ^(i). 

Intuitively  the  existence  of  h  implies  that  /  can  be  continuously  deformed  into  g 
without  leaving  Y. 

The  homotopy  relation  between  mappings  from  X  into  Y  is  an  equivalence 
relation  on  the  function  space  Y'^.  Hence  the  homotopy  relation  partitions  Y’' 
into  disjoint  equivalence  classes,  which  are  called  homotopy  clasRes.  We  write  the 
homotopy  relation  as  f  g.  Those  homotopy  classes  capture  our  intuitive  notion 
of  classes  of  paths.  The  homotopy  cla.sses  of  Y^  can  be  shown  to  be  precisely  the 
arcwise-connected  components  of  Y^  (Hocking  and  Young  (1961)  ). 


To  take  a  concrete  example,  consider  configuration  space  for  the  two-dimensional 
mover’s  problem  to  be  the  product  space  of  the  2-dimensional  Euclidean  plane  JJ?"* 
and  the  one-dimensional  sphere  S'  to  obtain  X  S',  and  denote  the  configuration 

obstacles  as  CO  C  3?"  X  S'.  Now  two  paths  /  and  g  in  the  same  equivalence  class 

must  belong  to  a  parameterized  family  of  mappings  sucli  that: 

/i  :  X  X  /'  -►  X  5'  -  CO. 

and  fi{i,  0)  =  /(^)i  1)  =  before. 

Now,  let  y  be  a  point  in  Y.  The  y  neighborhood  of  cyclic  paths  in  Y,  C[Y,y) 

is  the  collection  of  all  continuous  mappings  f  :  —*  Y  such  that  /(O)  —  /(I)  =  y, 

i.c.,  the  set  of  all  continuous  curves  that  begin  and  end  at  y.  If  /  and  g  are  curves 
in  C[Y ,  y),  we  say  that  /  is  homotopic  to  g  modulo  y  if  there  exists  a  homotopy 
/i  :  X  ►  T  continuously  deforming  /  into  g  without  leaving  Y. 

Clearly,  homotopy  modulo  y  is  an  equivalence  relation,  and  decomposes  C[Y,y) 
into  disjoint  equivalence  classes  which  are  exactly  the  arcwise-conncctcd  components 
of  C{Y,y).  The  set  of  these  equivalence  classes  is  termed  the  first  homotopy  group, 
or  fundamental  group  of  Y .  We  say  a  path-connected  space  Y  is  simply -connected 
if  the  fundamental  homotopy  group  for  X  is  the  trivial  group  of  one  element 
(for  some,  and  hence  for  all  y  in  Y).  See  also  appendix  111,  section  “Topological 
Constraints.” 

11,2.  The  Connectivity  of  Configuration  Space 

The  configuration  space  X  5*  (for  the  two-dimensional  mover’s  problem) 
is  not  simply-connected,  since  5'  is  not  simply-connected.  The  function  space 

X  5')'  contains  several  homotopy  classes.  (3?^  X  5')'^  may  be  envisioned  as 
a  cylinder  on  which  there  are  clearly  two  classes  of  paths;  those  that  bound  a 
2-dimensional  region  and  are  contiactable  to  a  point,  and  those  that  go  around  the 
cylinder. 

The  configuration  space  3?'  X  50(3)  is  not  simply  connected,  because  SO(3) 
is  not  simply  connected.  To  see  this  consider  the  following:  geometrically,  50(3) 


is  hoincomorphic  to  P'',  the  3-sphere  with  antipodal  points  identiCcd.  As  is  well 
known  (see  Massey  (1967),  p.  166)  the  rundamcntal  group  for  P“  is  cyclic  of  order 
2,  and  hence  P”  is  not  simply  connected. 

General  configuration  spaces  (other  than  that  for  the  classical  Mover’s  problem) 
are  not  always  simply-connected.  For  example,  the  C-Space  for  a  mani[)ulator  with 
six  revolutc  joints  is  the  6-torus,  5’  X  5*  X  •  •  •  X  5’  (to  6). 

Let  n  be  the  half-open  interval  (— 7r,5r).  IT  can  be  used  to  approximate  5*, 
if  we  are  willing  to  tolerate  singularities  in  the  representation.  It  is  instructive  to 
generate  a  conCguration  space  which  is  simply-connected.  Since  this  is  not  possible 
for  the  general  product  space  5R"  X  5*  we  will  instead  consider  the  product  space  of 
3?"  and  II.  Thus  for  the  two-dimensional  mover’s  problem  we  consider  the  product 
space 

c  =  X  n. 

For  a  manipulator  with  m  revolute  joints,  the  C-Space  may  be  approximated 
by  n*”,  where 

n"*  =3  n  X  n  X  . . .  X  n  (to  m). 

The  three  dimensional  rotation  group  50(3)  can  be  approximated  by  a 
hemisphere  of  the  3-sphere  (which  is  simply  connected),  or  by  11^.  FI"'  is 
homeomorphic  to  the  interior  of  the  m-cube.  This  new  product  space  C  is  simply 
a  restricted  configuration  space  where  the  piano  is  not  allowed  to  spin  around 
wildly.  The  approximation  of  50(3)  by  a  hemisphere  of  5'^  incidentally,  is  closely 
related  to  the  employmciit  of  unit  quaternions  to  represent  rotations.  The  space  of 
unit  quaternions  is  precisely  5'*;  the  two  quaternions  q  and  —q  construct  the  same 
rot.ition,  although  they  represent  antipodal  points  on  5  *.  When  all  antipodal  points 
q  and  —q  are  identified,  the  projective  3-sphcre,  is  obtained.  P^  is  isomorphic 
to  50(3).  It  is  of  interest  that  Euler  angle  space,  Q*  (sec  chapter  2)  is  essentially 
an  approximation  of  50(3)  by  SR'*  (mod  27r),  which  is  isomorphic  to  the  3-torus, 


when  equivalent  rotations  arc  idcntiricd.  If  equivalent  rotations  arc  not  identified, 
then  Q*  is  isomorphic  to  SR^.  While  the  approximation  of  SO(3)  by  9?'*  yields  a 
simply-connected  configuration  space,  from  the  point  of  view  of  an  automated 
planner  it  has  the  undesirable  effect  of  introducing  an  infinite  number  of  goals  in 
the  rotational  dimensions  of  configuration  space,  for  every  single  goal  in  the  space 
of  Euclidean  motions.  For  this  reason  the  approximation  provided  by  the  3- torus 
may  be  considered  preferable. 


Appendix  HI 


Integrating  Local  and  Global  Algorithms  for  the  Find-Path  Problem 

In  Donald  (1983a),  we  have  discussed  the  integration  of  a  global  channel 
algorithm  with  a  local  C'-space  algorithm  to  form  a  planning  system  for  the 
find-path  problem  in  X  S^.  flow  can  a  three  dimensional  global,  or  channel 
algorithm  be  coupled  with  the  planning  system  in  9?’^  X  50(3)  described  in  previous 
chapters?  More  generally,  what  are  the  fundamental  issues  in  integrating  local  and 
global  geometric  planning  algorithms?  In  particular, 

(i)  How  can  a  global  algorithm  suggest  paths,  or  equivalence  classes  of  paths  to  a 
local  algorithm? 

(ii)  Hov/  can  the  relevant  geometric  constraints  be  identified  and  exploited  by 
the  local  algorithm?  Conversely,  how  can  irrelevant  geometric  constraints  be 
effectively  ignored? 

(iii)  How  can  global  topological  constraints,  such  as  those  arising  from  analysis 
of  homotopy  classes  and  fundamental  groups,  be  propagated  onto  the  (local) 
geometric  structure  examined  by  the  local  algorithm? 

In  general,  the  design  of  a  global  algorithm  will  depend  on  the  geometric 
constraints  exploited  by  the  companion  local  algorithm  with  which  it  will  be 
coupled.  Hence  when  we  consider  extending  the  channel  algorithm  of  Donald 
(1983a)  to  the  three  dimensional  find-path  domain,  we  must  specify  what  “target,” 
local  algorithm  to  use.  A  natural  candidate  i.s  the  local  algorithm  for  lind-path  in 
9?'*  X  50(3),  which  we  describe  in  chapters  1  2. 


Path  Suggestion.  A  problem  which  must  be  solved  in  any  local/global  find- 
path  integration  is  how  a  global  path  may  be  suggested  to  a  local  algoritlnn.  In 
two  dimensions  this  was  accomplished  by  segmenting  the  find-path  problem  into 
a  sequence  of  sub- problems.  The  Suggestor  strategy  (chapter  2)  is  designed  with 
this  in  mind.  The  verified  points  along  the  suggested  path  become  planning  islands 
in  configuration  space.  The  job  of  the  local  algorithm  is  then  to  connect  up  the 
planning  islands  and  find  a  continuous  path  to  the  goal. 

Choosing  Subgoals  in  Rotation  Space.  In  a  three-dimensional  rotation  space, 
the  problem  of  selecting  good  rotational  subgoals  becomes  more  difficult.  Much 
of  the  path-planning  literature  has  been  guilty  of  overlooking  this  difficulty.  Such 
subgoals  can  be  used  in  path -suggestion  as  we  have  described  above.  Even  when 
the  companion  local  algorithm  is  complete,  a  strategy  for  choosing  good  rotational 
subgoals  is  desirable,  since  it  would  allow  the  algorithm  to  converge  faster. 

We  have  derived  experimental  strategies  which  consider  alignments  of  the  robot 
polyhedra  with  the  faces  and  edges  of  obstacles.  Every  polyhedron’s  boundary 
contains  alignablc  generators  (faces  and  edges)  which  have  an  orientation,  and 
non-alignable  generators  (vertices)  which  have  no  orientation.  Two  generators  are 
said  to  be  aligned  when  they  are  parallel,  and  the  rotations  in  which  they  are 
aligned  form  connected  alignment  regions  in  50(3).  For  example,  two  ciibes  are 
aligned  when  two  faces  are  parallel,  or  an  edge  and  a  face  are  par.illcl.  The  channel 
construction  is  useful  for  identifying  the  obstacle  surfaces  which  bound  the  proposed 
channel  (in  real  space).  Call  this  set  of  faces  Tic-  Let  7i{  denote  the  faces  of  the 
robot.  The  alignment  regions  can  be  considered  for  the  generator  pairs 

G Align  =  (^^11  X  X  X 

We  arc  only  interested  in  applicable  alignments,  that  is,  an  alignment  of  two 
(or  more)  generators  where  the  generators  can  be  brought  into  contact  through 
some  translational  motion  (this  is  our  definition  of  applicability:  see  chapter 
3).  Applicability  may  be  determined  by  examining  the  applicability  constraint 
functions  (ACFs)  introduced  in  chapter  3.  Furthermore,  in  chapter  5,  we  showed 
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that  alignments  of  edges  and  faces  occurred  exactly  at  the  boundary  of  the 
applicability  regions  for  C-functions. 

Every  C-function  is  a  partial  function  fi  on  the  configuration  space  9?'*  X  SO{3), 
whose  domain  is  3?'^  X  A{  for  Ai  C  50(3).  The  set  of  alignment  regions  is  obtained 
by  the  union  of  the  boundaries  of  these  applicability  regions 

Ralign  =  U  ^Ai 

t 

for  every  C-function  Every  point  in  Raiign  in  the  kernel  of  some  ACF 
gi  j  :  50(3)  SR  for  a  C-function  In  chapter  4  we  showed  how  to  derive  charts 
for  these  boundaries  and  their  intersection  manifolds. 

In  most  find-path  problems,  the  alignment  regions  in  Raiign  ^^e  complete  as 
subgoals  in  rotation  space — i.e.,  no  other  rotational  subgoals  need  be  considered  in 
order  to  find  a  solution  path  (i**  one  exists)  This  makes  a  certain  intuitive  sense: 
one  might  try  aligtiing  a  large  box  with  a  narrow  door-frame  in  order  to  squeeze 
it  through.'  However,  in  general  there  exist  pathological  cases  in  which  this  is  not 
true  (imagine  a  robot  which  looked  like  a  polyhedral  sea  urchin),  and  the  set  of 
alignments  is  not  complete  as  a  set  of  subgoals.  Furthermore,  it  is  unsatisfying  that 
the  alignment  analysis  exploits  strong  constraints  in  the  polyhedral  domain  of  the 
classical  Movers’  problem,  and  does  not  appear  to  generalize  well  to  linked- arm 
problems. 

We  believe  that  it  may  be  possible  to  overcome  the  problem  of  “star-shaped”  robots 
by  considering  additional  alignment  regions  obtained  from  faces  and  edges  of  the 
convex  hull  of  such  objects.  Such  an  algorithm  would  have  to  deal  similarly  with 
“star-shaped”  obstacles.  Even  these  additional  alignments  may  prove  incomplete; 
however,  they  may  have  heuristic  value. 

The  problem  of  how  a  global  algorithm  can  infer  good  rotational  subgoals  from 
the  structure  of  real-space  is  one  of  the  most  interesting  open  problems  in  spatial 
planning.  We  conjecture  that  an  answer  may  lie  in  the  structure  of  the  boundaries 
'We  are  iiol.  inakiiig  nny  claims  alxxil  liiiitian  »|ial.ial  re.-Lsoniiig  here. 


of  the  applicability  regions  in  SO(3).  The  algorithms  we  provide  in  chapter  5  for 
obtaining  the  applicability  set  decomposition  may  prove  useful  in  computing  this 
structure.  Such  a  planner  might  slide  along  the  intersections  of  ACF  boundaries 
or  level  ACFs  in  rotation  space, ^  much  as  our  planner  slides  on  the  intersections 
of  level  C-surfaces  in  conBguration  space.  The  advantage  of  such  an  approach  lies 
in  reducing  the  (infinite)  search  for  rotational  subgoals  to  a  finite  combinatorial 
search  along  the  ACF  boundaries. 

Topological  Constraints.  Ideally,  the  global  strategy  should  enforce  the 
path-class  criterion'^  for  each  sub-problem:  no  straight-line  approximation  for  a 
subproblcm  may  cross  more  than  one  equivalence  class  of  paths.  We  begin  by  defining 
what  a  straight-line  approximation  means  in  3?^  X  50(3).  This  requires  some  way  of 
forming  “linear  combinations”  of  rotations.  The  definition  of  “linear  combinations” 

■bc'vc!l  A('I''.h  jire  (l(Tiiic'<l  in  at-clion  3.11. 
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in  SO(3)  relates  to  a  dcGnition  of  convexity  for  9i‘*  X  50(3).  In  particular,  we  wish 
to  determine  whether  C-space  obstacles  are  convex.  For  if  C-space  obstacles  are 
convex,  then  wc  could  trivially  bound  the  number  of  intersections  any  straight-line 
trajectory  can  make  with  any  one  obstacle.  We  present  a  conjecture  that  the 
C-space  obstacles  are  non-convex.  Finally,  wc  discuss  basic  topological  notions  for 
formalizing  our  analysis  of  equivalence  classes  of  paths. 

We  require  some  way  of  forming  “linear  combinations”  of  rotations.  The 
requisite  algebraic  structure  is  much  like  a  module  (over  the  reals),  except  that 
the  group  operation  cannot  be  commutative.'*.  The  group  operation  is  composition 
of  rotations.  Let  Jl{h,  6)  denote  rotation  about  the  three  dimensional  vector  n  by 
angle  0.  Scalar  multiplication  by  a  6  may  be  defined  by 

aJl{n,0)  =  Jl(n,aO). 

By  substituting  the  (non-commutative)  composition  of  rotations  for  the  group 
operation  -f,  we  obtain  a  natural  dclinition  for  linear  combinations, 

a;?(n,0)-f  (I  -a)je(n',fl')  =  R{n,aO))l{n',{l  -  a)0')  (///.I) 

for  0  <  a  <  1.  of  course,  may  be  conveniently  expressed  by  a  unit  quaternion. 

Suppose  is  a  three-dimensional  parameter  space  for  50(3) — that  is,  the 
domain  of  a  chart  for  rotation  space.  For  example,  might  be  the  space  of  Euler 
angles  (see  chapter  2).  It  is  possible  to  define  linear  combinations  in  the  parameter 
space  3?^  X  Q'^.  This  seems  unsatisfactory,  since  it  makes  the  definition  of  linear 
combination — and  more  disturbingly,  of  convexity — dependent  upon  the  chosen 
parameterization  for  50(3).  Observe  that  definition  (lU  l)  for  linear  combinations 
is  invariant  for  all  parameterizations. 

Open  Question:  Under  a  definition  of  convexity  invariant  for  all  parametcriza- 

tions,  show  whether  or  not  the  C-space  obstacles  in  3?'*  X  50(3)  (and  3i”'^  X  5*) 

'’Recall  that  a  rtiodulc  is  (Jcliiied  a.s  follow.s;  If  II  i.s  a  rmniimtalivo  riiif'  with  iilrtilily,  then  M 
is  a  module  over  II  if  (Al,4)  is  a  cotiitiiiitativc  nroii(>,  and  scalar  iinilliplic.itioii  rM  of 

clcineiits  M  in  At  by  r  in  l{  is  associative  and  <listributive  (over  +),  and  if  —  M 


are  convex.  Conjecture:  We  conjecture  that  C- space  obstacles  are  non-convex. 
When  3f”  X  -S'*  is  approximated  by  9?“^  X  [— tt, tt)  and  embedded  in  9?^,  the  cor¬ 
responding  C-space  obstacles  arc  non-convex  using  Euclidean  linear  combina¬ 
tions.  Furthermore,  in  both  9?^  X  and  9i'^  X  SO(3),  each  obstacle  manifold 
is  the  intersection  of  a  finite  number  of  half-hyperspaces  of  9?'*  X  50(3). 
Each  half-hypcrspace  is  in  turn  defined  via  a  real-valued  partial  function 
/  :  9?3  5^)(3)  ^  9?.  fJsing  partial  functions,  arbitrary  non-convex  manifolds 

can  be  constructed.  Showing  that  the  obstacle  manifolds  could  be  represented 
by  means  of  smooth,  total  functions  would  suggest  convexity.  Our  analysis 
suggests  that  these  functions  must  be  partial,  which  in  turn  leads  to  the 
conjecture  that  the  obstacle  manifolds  are  non-convex.  | 

The  hornotopy  relation  (see  appendix  II)  partitions  the  function  space  of  paths 
into  ccpiivalencc  classes.  The  image  of  one  such  equivalence  class  [/]  is  the  region 
in  C- space  covered  by  the  union  of  all  the  path  images  in  [/].  The  equivalence 
classes  are  determined  both  by  the  structure  of  the  underlying  C  space  ,  and 
by  the  C-space  obstacles.  Intuitively,  the  fundamental  group'’  in  a  space  "K  is  a 
topological  invariant  corresponding  to  the  set  of  equivalence  classes  of  paths  in 
Y .  The  group  operation  corresponds  to  path  composition  (“pasting”),  and  for  two 
paths  f ,  g  :  Y  where  /(l)  =  S'(O). 


f  *9it) 


im), 


for  t  G  [0,  2] 


\3(2t-l),  fort  €[5,1]. 


We  think  of  /  *  g  as  the  path  whose  first  half  is  /  and  whose  second  half  is  g. 
The  pasting  operation  *  is  well  defined  on  path  hornotopy  classes: 


[/]  *  [ffl  =  (/*?]. 

and  cxliibits  groupoid  properties.  When  an  obstacle  makes  a  hole  in  frec-space,  it 

augments  the  fundamental  group  for  the  space  by  adding  an  infinite  (cyclic)  number 

■’Sec  ,'ip|)cti(lix  II  for  a  review  of  elemenl.'iry  hornotopy  theory  and  a  formal  delinitioii  of  the 
fijnda[[ieiilal  group. 


of  equivalence  classes.  (For  example,  by  puncturing  the  plane  at  the  origin  O,  we 
obtain  the  classes  of  paths  (1)  not  looping  around  O,  (2)  looping  around  O  once, 
. . . ,  (n)  looping  n  —  1  times  around  O,  . . . ).  The  topology  of  the  underlying  C-space 
may  be  predetermined  (see  appendix  II),  but  each  new  find-path  environment 
generates  different  path  homotopy  classes.  We  wish  to  infer  the  equivalence  classes 
in  the  fundamental  group  by  their  generators,  i.e.,  the  C-space  obstacles.  Since  the 
C-space  obstacles  can  be  constructed  from  the  real-space  obstacles,  we  are  actually 
attempting  to  compute  path  classes  in  C-space  from  the  structure  of  real-space. 

In  general,  if  frcc-space  is  connected,  the  image  in  C-space  of  even  a  single  class 
of  paths  can  cover  all  of  free-space.  However,  we  can  impose  a  stronger  condition 
which  subsumes  the  path-class  criterion.  Let  p  be  an  injection  of  /’  into  C-space 
,  which  will  represent  some  approximation  of  a  solution  path  for  a  subproblem  E. 
We  wish  to  know  whether  the  image  of  p  can  be  expressed  as  the  union  of  two 


sets  of  points,  those  lying  in  the  image  of  one  equivalence  class  of  paths,  and  those 
lying  in  C-space  obstacles  bounding  that  image.  In  chapter  4,  we  showed  how  to 
intersect  trajectories  with  C-surfaces  in  5?^  X  and  X  SO{3).  We  can  intersect 
p(I^)  with  all  C-surfaccs,  and  determine  the  intersection  points.  These  intersection 
points  indicate  where  it  penetrates  C-space  obstacles,  and  are  determined  from  its 
zero-crossings  from  frec-space  to  forbidden  space. 

Claim:  That  the  image  of  p  contains  either  no  region  or  one  connected  region 
lying  within  any  C-space  obstacles  is  a  suflicient  (but  not  a  necessary)  condition 
satisfying  the  path-class  criterion  for  sub-problems. 

Constraint  Relevance.  Another  issue  concerns  how  a  global  algorithm  can 
characterize  the  relevant  constraints  for  a  local  algorithm,  and  if  necessary,  impose 
additional,  artificial  constraints  on  the  problem  so  that  irrelevant  constraints  in 
the  initial  domain  will  be  ignored.  Because  of  the  dilTiculties  in  maximizing  channel 
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breadth  in  three  dimensional  channel  construction,  the  artificial  faces  introduced 
by  the  current  implementation  may  prove  too  restrictive,  especially  if  the  robot  is 
large  or  non-convex.  However,  the  channel  con«( fiction  is  useful  for  identifying  the 
obstacle  surfaces  which  bound  the  proposed  cn  .  nel  (in  real  space).  Cali  this  set  of 
faces  Jj(.  The  C-surfaces  generated  by  these  faces  may  be  exploited  by  the  sliding 
and  mtersech'on  experts  (see  chapter  2).  Let  Tu  denote  the  faces  of  the  robot.  These 
preferred  C-surfaces  are  identiOed  with  their  generator  pairs,  namely 

Slide  =  X  vert[7ic'^\j{hju  X  d7K^\j(vert{7]{)  X 

The  identiCcation  of  good  C-surfaces  to  slide  along  addresses  a  central  issue  in 
local/global  integration.  At  present,  the  sliding  and  intersection  experts  exploit  only 
local  geometric  structure  and  planning  history.  The  channel  algorithm  introduces 
a  global  criterion  for  selecting  which  C-surfaces  to  slide  along,  and  for  constructing 
intersection  manifolds.  The  set  of  C-surfaces  Slide  specifies  an  implicit  volume  in 
configuration  space  which  is  closely  related  to  the  channel  volume  (in  C-space  ). 
This  volume  is  obtained  by  extending  the  hypcrplanes  containing  the  C-surfaces 
past  the  obstacle  boundaries  until  they  intersect.  Furthermore,  Slide  lies  on  the 
boundary  of  the  image  of  the  Jiypothesized  equivalence  class  of  paths.  By  choosing 
these  interior  surfaces  as  candidates  for  the  sliding  and  intersection  experts,  global 
advice  on  constraint  relevance  is  provided  to  the  local  C-space  algorithm  by  the 
global  channel  algorithm. 


Appendix  IV 

A  Listing  of  Maesyna  C«)de 

In  this  appendix,  we  provide  a  listing  of  the  Maesyma  code  to  produce  optimized 
Lisp  procedures  for  computing  the  coefficient  functions  of  the  canonical  linear 
forms  and  trigonometric  quadratic  forms  of  the  type  (a),  (b),  and  (c)  C-functions, 
and  type  (a)  and  (b)  ACF  clauses  for  5?^  X  SO(3).  Using  these  forms,  tlic 
intersection  manifolds,  type  (c)  ACFs,  and  disambiguating  applicability  constraints 
are  constructed  in  the  manner  described  in  the  text. 

We  also  list  (in  Maesyma)  the  resulting  combined  forms  for  tlie  (f-functions  and 
ACFs.  Note  tliat  the  type  (c)  C-function  is  “over  a  page  long.” 

Rotations  are  speciRed  by  means  of  the  Maesyma  functions  RotateV cc.tor[x  : 
vector)  and  '1' cans f orrn{x  -.plane  or  vector).  Rotations  are  implemented  using 
(lie  Ruler  angles.  Howevcu'-,  by  changing  these  two  functions,  any  representation 
for  rotations  such  as  (luatern ions,  spherical  angles,  or  wrist  angles  for  a  cartesian 
manipulator  may  be  employed,  d'his  corresponds  to  reparamel  I'rizing  .S'O(d),  and 
results  in  different  charts  I'or  the  level  CRsurfaces,  intersection  manifolds,  and  AC'F 
manifolds. 
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/•  Display  and  Grind  function.  If  OPTIMIZE.FIAG  is  TRUE  than 
wa  actually  store  the  0P11M12ED  expression  */ 

pr1nt(*Sett1ng  Grind  to  TmiE...”)S 
Gr1nd:TrueS 
Optlmize.nag:  TRUES 
/•  OPIMPREFIX;  X  •/ 

Dlsp1ay_Bnd_6r1nd(exp) :• 

B1ock([labe1]. 

If  0PIIMI2£_fLAG  THEN 
(prlnt(tXP) . 

Pr1nt{'  Optimizing...*). 

Exp:  Opllnlze(Exp)), 
label  ;t.disp(exp). 

If  Grind  then 

(pr1nt(’  Ground,  becomes: 

*.  labBl[l].*  :  •). 

grind(exp)). 
print(*  ’). 
labe1[l])S 


/•  Utility  function.  Is  the  expression  EXP  free  of  all  the  VARS  (a  list?)  •/ 

f ree_Of_Vars(Vars.  Exp)  :• 
b1ock([freedon], 

freedom: true, 

(for  var  In  Vars  unless  freedom  •  falsa  do 

(if  Not  FraeOf(Var,  Exp)  then  freedom:  falsa)), 
if  Not  freedom  then 

Print("[Exp  contains  Major  Variables.  Recurslwely  Analyte. . .]")• 
freedoffl)S  . 

/*  here  we  define  Canonical  Linear  Form  to  be  simply  the 
expression  of  the  constraint  as  a  linear  function 
In  X.  Y.  and  2  •/ 

Canon1ca1_11near.var1tb1es:  [X,  Y.  Z]$ 

Canonfca1_1 1near_form(Exp)  :• 

Iso1ateN(Exp ,  Canonical.! 1near_var1ab1at)$  . 
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/•  Bruc*  Donald  (BR0B02)  analyaa  hairy  txpressions  HACSVMA 
a  littia  bit  •/ 

/•  Analyza  Bilinear  Forms  :  jiven  the  "chief  »ara"  in  RATVARS. 
generate  intermediate  labels  for  all  the  coeffieiantt 
of  these  vars  and  return  the  "simplified"  bilinear  form. 
Recursively  Calls  ANALY2[_BUIMEAR_fORM  So  that  the  intermediate 
labels  are  truly  "constants"  relative  to  the  RatVars  •/ 

/•  typically.  ratvars:[».y.*.p$i. theta. phi3  •/ 

/•  IsolateN  works  like  ISOLATE  but  for  N  variables  in  a  list, 
on  a  bilinear  form  •/ 

lsolateN(E»p.  Bvars)  :• 

Block([Save_Ratvars.  Iform]. 

Save_ratVars;  ratwars.  ‘ 

RatVars:  Hvars. 

,  Iform:  Ar.alyre_Bil  inear_form(Esp) . 

Ratvars:  Save_Ratvara, 

Iforn)S 

sinple_Analyze_depth:  4S 

Analyze_Bilinear_rorm(esp)  :• 

block([poi.er.  Coef.  Rat_E»p.  Left.  lose.  Sum.  Label]. 
print("Analyzing:"). 

1 disp( reveal (Exp. I imple_analyze_depth)). 

Left:  rat(e»p).  Sura;  0. 
for  var  in  Ratvars  do 

(Power:  Hipow(Lsft,  Var). 
if  power  >  2  then 
(lose:  var'power. 

Error("Warning:  Not  a  Bilinear  Form  because  of  , 
Coef:  ratcoeff (Left,  var,  2). 
if  not  (Coef  •  0)  then 

(  Print(  "The  coefficient  of  ".  var*2."  is  *). 

Label :Oisplay_and_grind(Coef). 
if  not  Free_of_vars(ratvars.Coef) 

then  Label:  Analyze_Bilinear_form{Coaf). 

Sum:  Sum  ♦  label  •  Var*2. 

Left:  rat(left  -  Coef  *  (Var  ‘2)))). 
print("N1*ed  terras:  *), 
for  var  in  Ratvars  do 
(for  var2  in  Ratvars  do 
if  var  «var2  then 

(Coef:  ratcoeff(Left.  var»var2.  1). 
if  not  (Coef  •  0)  then 

(  Print(  "The  coefficient  of  ",  Var»var2.”  is  ). 
label :Oisplay_end_grind(Coaf), 
if  Not  free_Of_vars(RetVar$,  Coef) 

then  Label:  Analyze_Bil ineer_form(Coef ) . 

Sum:  Sura  ♦  label  •  var*  var2. 

Left:  rat(left  *  Coef  •  (Var  •  ver2))))). 
print("  Linear  Terms:  "), 
for  var  in  Ratvars  do 

(coef;  ratcoeff(left,  var,  1). 
if  not  (coef  •  0)  then 

(  print('The  Coefficient  of  ".var."  is  "). 
label : Displ ay_and_9rind(Coef ) , 
if  Not  Free_0f_v8r$(RatVart.  Coef) 

then  Label:  Analyze_Bi1inear_form(Coaf ), 

Sum;  Sum  ♦  label'ver. 

Left:  rat(lefl  -  Coef*  Var))), 
if  Left  #  0  then 

(Print("  And  the  constant  term  is  "). 

Label:  Diipley_and_9rind(left). 
sum:  Sun  *  Label ) , 

Pr1nt("  Yielding  :").  Oi$play_and_grind(Sum) ,  Sum)$ 


/•  Bruce  R.  Doneld.  (BRD902)  ModeiMacsyma 

Attempt  to  express  applicability  constraints  for 
C-surfaces  in  R*3  Across  S‘3  •/ 

/•  PRODUCTION  VtRSlON  --  i.e.,  for  production  of  llSP  code  •/ 

BTHETA:  [phi.  theta,  psi]S 

if  .Eu1er_Rolation_equations_1oaded  ■  TRUE  then  *0K* 
else  iatchioad  ([rotate. mac]) : 

shorten(exp) : •  subst(  S.sin,  subst(C,  cts.  exp))$ 

ralvars:[  s in{ phi ) .cos(phi ) .sin( theta) .cos( theta) .  sin(psi) .cos(pti)]; 

/•  Each  constraint  is  of  the  form  •/ 

/•  vectors:  •/ 

u(1)  ;•  [ux[i],uy[i].u2[i]]: 
v(i)  :•  [vx[i],wy[i],v2[i]]; 

/•  Normal  for  pTane  eq  •/ 

0(i)  :■  [nx[i],ny[i].n2[i].nd[i]]; 

/•  Here  ee  define  functions  to  generate  the  appT icabi I ity  conitraints. 
the  arguments  are:  Bn  :  a  vertex  in  3-space,  which  we  use  to 

measure  distance  to  the  pTane. 

V  :  a  vertex  which  we  insist  must  be  ON  the  plane. 
N  ;  a  plane  (4-vector) 

c  :  the  "height"  of  the  Tevel  surface  in  S"3. 
if  0,  corresponds  to  the  maximim  boundary 
of  the  applicability  clause  (eg  edge-face 
contact). 


R3_projection(Vec)  [vec[l].  vee[2].  vec[3]]$ 

Type_B_Clause(N.  bn.  v.  e)  :• 

simp_3( 

(R3_Projeetion(N)  .  Rctate«vector(Bn)) 

-  (R3_Projection(N)  .  Rotate_vector(v)) 

-  c); 


Type_A_e1aute(N,  bn.  v.  e)  :• 

Block ([N_THETA], 

N_THETA:  part( transform(Euler_Inverse.  M).  1), 


sifflp_3( 

(R3_ProJection(N_THETA)  .  bn) 
-  (R3_Projection{N_theta)  .  ») 

-e)); 


Midp(a.b)  :•  a  ♦  (b-a)/Z$ 


Type_Cl_Claose(nl.  n2.  al.  a2.  cl,  c2)  :• 
simp_3( 

-  type_b_clause(nl.  al.  midp(al.a2).  el) 

•  type_b_ciause(n2 ,  al.  Midp(al,a2),  c2)); 


Type_C2_Clause(Hl.N2.  bl,  b2,  cl.  c2)  :■ 
s1mp_3( 

-  typc_a_c1auta(nl ,  bl.  eiidp(bl .b2) ,  cl) 

•  type_a_clause(n2 .  b2.  miop(bl.b2).  e2)); 


/•  lav[1]  is  just  a  CONSTANT  to  construct  a  Level  surface  on  S*3  which 
is  spplicable  */ 


/•  KC.  VC.  ZC.  WC  tre  Coordin«t#* iec^ssor  functloni  (MACROS)  In  LISP  •/ 


/*  bruca  r.  donald.  cspace  constraints  in  3-dinansiont.  **-  aacsyma 


/•  production-version:  i.e.  produce  lisp  code... 

ac.  yc  and  zc  are  baccessor  macros  for  components  of  vectors  */ 

/*  this  neat  section  contains  the  equations  for  c-surfaces  in 
r*3  a  s*3.  see  aim  60S,  tonas'  spatial  planning  paper  for  details. 

/*  load  eu1er  rotation  equations  ■/ 
if  eu1er_rotation_equations_1oaded  •  true 
then  *ok* 

else  (batchload(''usrdS:[brd.prod]rotate.mac*), 
eu1er_rotation_equat ions.loaded; true) : 

/•  a  vector  in  r*3:  •/ 

avec  :  [a,  y,  z]; 

ratvars:  [a,  y,  z, 

sin(phi),  cos(phi),  sin(  theta),  cos(theta). 
tin(psi).  cos(pti)  ]; 

/*  a  vertaa  on  a  ,  and  a(i-»l)  */ 

ai  :  [ac(ai)  .  ye(ai).  zc(ai)]; 
aione:  [ac(aione),  yc(aione).  zc{aiona)]; 

/*  a  vertaa  on  b  ,  and  b(j>l)  */ 

bj:  [*e(bj).  yc(bj).  zc(bj)]; 

bjone;  [ac(bjone),  yc(bjone),  zc(bjona)]: 

/*  the  normal  to  a  face  fi  on  a  */ 

nfi:  (  ac(nfl).  yq(nfi),  zc(nfi),  «c(nfi)]; 

/*  the  normal  to  a  face  gj  on  b  */ 

ngj  :  CKC(ngJ).  ycCngj).  zc(ngj).  ecCngJ)!; 

/*  type  a  surface,  rotate  the  normal:  */ 

l*3_proJection(Vec)  :•  [vectl],  vec[3],  vec[3]]S 

Rot_Nfi;  part(Transforffl(6u1er_Inverso,nfi).l): 

»l«a:  •l3_ProJection(Rot_llf1); 

Inner_Product_Term:  (Rotate_vector(a1}  *  bJ); 

A_6:  R_a  .  Xvec  -  (R_a  .  Inner_Produet_Term) ; 

/•  Type  B  Surface  •/ 

B.b:  R3_proJact1on(HgJ); 

B_6  :  ll_b  .  Xvec  -  (ll_b  .  Innar_Product_Term)  ; 

/•  Type  C  surface  (I)  •/ 


Cdge.a:  rotate_vector(aione)  -  rotata_vector(ai); 
Edge.b:  Bjone  -  bJ: 

H_C:  Crots(  Edge.a,  Edge.b); 

C«6:  ll_C  .  Xvec  -  (B_c  .  lnner_Product_Term) ; 


/•  simplify  if  postibitf  •/ 


Simp_l(t»p.  Var)  ;■  ratsubtt(l,  ain(«ar)*2  ♦  cos(var)*2,  aip); 
*imp_3(rapj  :•  R8t(Simp_l(  Simp_l(  Simp.l(Iap.  Phi),  Thfta).  Pli)); 

*_6;  Sinp_3(rat*ifflp(»_5)); 

B_5:  Simp_3(ral$inp(B_6)); 

C_5:  Sinp_3(patsimp(C_S)); 

[1enath(a>5).  length(b_5),  1en9th(C_5)]; 

/•  Grind  the  forms  here  •/ 

Prime 

Iype_A_Csurface[l]  S  9rind(A_5)$ 

Prime 

T Jlpe_B_Csurface[l]  :")$  6rind(B_5)$ 

Prime 

T>pe_C_Csurface[l]  :")$  6rind(C_5)$ 
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/•  MODfr  MACSYMA 

(HtRl  ARt  1HE  DdINIIIONS  Of  TYPE  (A)  (B)  AND  (C)  C-SURfACtS  fOR 
THE  6D0F  MOVERS  PROBLEM.  AND  TYPE  (A)  AND  (B)  APPLICABILITY 
CONSTRAINTS  .  Output  of  CSPACE  And  APPLIC  under  PRODUCE  for 
production  run. )  */ 


Type_A_Csurf»ce[l]  : 

(((XC(Nri)»X-XC(BJ)*XC(Nri))‘COS(PHI)»(XC(Nri)*Y-YC(BJ)»XC(NFI))»SIN(PHI)) 

•COS{THETA) 

♦  (-XC(Nf  I)*/-‘ZC(BJ)»XC(Nri))*SI«(IHtTA)-*(YC(Nfl)»Y-YC(flJ)‘YC(Nri))*COS(PHl) 
♦(-YC(Nri )‘X-XC(BJ)«YC(Nri))*SIN(PHl)-YC{Nfl)) 

•COS(PSI) 

♦(((-YC(NFl)*X-XC(BJ)*YC(NFI))»COS(PHI) 

♦(-YC(Nf I )»Y*YC(BJ)»YC(NFI))*SIN(PHI)) 

•COS(THETA) 

-(YC(Nf  I  )*Z-2C(BJ)*YC(Nri))»SIN(THtTA)-.(XC(Nri)»Y-YC(BJ)*XC(Nri))»C0S(PHI) 
■*(-XC(Nf  I)*»*XC(BJ)*XC(NFI))*SIN(PHI}-XC(NfI>) 
•SIN(PSI)-*(ZC(NFI)*2-2C(BJ)*2C(Nf  I))*COS(TMETA) 

■‘((ZC(NFI)»X-XC{BJ)*2C(Nf  I))*COS(PHI)  +  (2C{NfI)»Y-YC(BJ)»2C{NFl))»SlN(PHI)) 
•SIM(IHETA)-ZC(AI)*2C{NFI)-YC(AI)«YC(NFl)-XC(Al)»XC(Nri)$ 


Type_B_C»urf«ce[13  ; 


((-XC{AI)»XC(NGJ)»COS(PHI)-XC(AI)*YC(NGJ)*SIN(PHI))*COS(THETA) 
♦XC{AI)*ZC(NGJ)«SIM( THETA)- YC(AI)«YC(«GJ)*C0S<PHI)«YC(A1)»XC(NGJ)»SIN(PH1)) 
•COS(PSl) 

+((YC(A1)»XC(NGJ)*C0S(PHI)»YC(AI)*YC(NGJ)*SIH(PH1))»C0S(THETA) 
-YC(AI)»2C(NGJ)*SIN(THETA)-XC(AI)»YC(NGJ)»C0S(PHI)+XC(AI)»XC(NGJ)*SIN(PHI)) 
•SIN(PSI)-2C(AI)»ZC(NGJ)*COS( THETA) 
•►(-ZC(AI)*XC(NGJ)»COS(PHI)-ZC(AI)«YC(NGJ)»SIN(PHl))*SIN(THETA) 
-YC(NGJ)»C0S(PHI)»XC(NGJ)»SIN(PHI)*ZC(NGJ)‘2*YC(NG0)*Y*XC(N6J)*X 
-2C(flJ)»2C{NGJ)-YC(BJ)»YC(NGJ)-XC(8J)*XC(N6J)$ 

Type_C_Cturfice[l]  ; 


(({((XC(AIONE)-XC(AI))*YC(BJONE)+(-XC(AIONE)*XC(AI))»YC(BJ))‘Z 

♦((-XC(AI0NE)*XC(AI))'ZC(BJ0NE)»(XC(AI0NE)-XC{AI))*ZC(BJ))*Y 

*(XC(AIONE)-XC(Al))*YC(BJ)*ZC(BJONE)*(-XC(AIONE)*XC(Al))'ZC(BJ)»YC(BJONE) 

♦(-YC(AI)»2C(AI0NE)-ZC(AI)»YC(AI0NE))*XC(BJ0NE) 

♦(YC{AI)»ZC(AIONE)-ZC(AI)»YC(AIONE))»XC(BJ)) 

•COS(PHI) 

♦(((-XC{AI0NE)*XC(AI))*XC(8J0NE)+(XC{AI0NE)-XC(AI))»XC(BJ))»2 

♦((XC(AI0NE)-XC(AI))«2C(BJ0NE)+(-XC(AI0NE)+XC(AI))*ZC(BJ))*X 

♦(-XC(AIONE)+XC(AI))*XC(BJ)»ZC(BJONE) 

♦(-YC(AI)*ZC(AIONt)*ZC(Al)»YC(AIONE))«YC(BJO«E) 

•»(XC(  ATONE  )-XC(AI))*ZC(BJ)*XC(BJONE) 
♦(YC(AI)»2C(AI0NE)-ZC(AI)»YC(AI0NE))*VC(BJ)) 

*SIN(PHI)«(XC(AIONE)-XC(AI))*ZC(BJONE)'»(-XC(AIONE)'»XC(AI))*ZC(BJ)) 

•COS( THETA) 

♦(((XC(AI0NE)-XC(AI))*XC(BJ0NE)+(-XC(A10NE)+XC(Al))»XC(BJ))»C0S(PHI) 
♦((XC(AI0NE)-XC{AI))*YC(BJ0NE)»(-XC(AI0NE)+XC(A1))»YC(BJ))»SIM(PH1) 
♦((-XC(AIONE)*XC(AI))»XC(PJONE)+(XC(AIOME)-XC(AI))»XC(BJ))*V 
*((XC(AI0NE)-XC(AI))*YC(BJ0NE)*(-XC(A10NE)*XC(AI))»YC(BJ))*X 
♦(YC(A1)»ZC( ATONE )-ZC{Al)«YC(AIONE))»ZC{BJONE) 

♦  (-XC{AIONt)  +  XC(AI))»XC(flJ)*YC(BJONE)-»(XC{AIOME)-XC(Al))»YC(BJ)»XC(BJOIIE) 
♦(-YC(AI)»2C(A10NE)*ZC(A1)«YC(A10NE))*2C(BJ)) 

•SIH(THETA) 

♦(((-YC(AIONE)+YC( AT ))*XC{BJONE)*(YC( ATONE )-YC(AI))»XC(BJ))»Z 

♦  ((  YC( AIONE )-YC( AT ))*2C(BJONE)*(-YC( ATONE )*YC(AI))»ZC(BJ))»X 
♦(-YC(AIONE)+YC(AT))*XC(BJ)*ZC(BJOHE) 

♦(XC( AT )»2C( ATONE )-2C(Al)»XC(AIONE))*YC(IJONE) 
♦(YC(A10Nt)-YC(AI))*2C(8J)*XC(BJ0NE) 

♦  (-XC( AT )»ZC( ATONE )*2C( AT )*XC(  ATONE) )*YC(eJ)) 


•COS(PHI) 

♦  (((-VC(AI0ME)»VC(AI))*VC(BJ0NE)-»(VC{A10tir)-VC(AI))»VC(BJ))»2 
♦((YC(A10NE  )-rC{AI)  )«ZC(BJOIir  )»(-yC{A10»(E  )*YC(AI )  )»EC(BJ)  )*r 
♦(-YC{A10NE)+VC{AI))«VC(BJ)*2C(BJ0I(E.)-‘(VC(AI0NE)-VC(A1))*ZC(BJ)»YC(BJ0BE) 

♦  (-XC1A1)*ZC(A10»E)+2C(AI)»*C(A10NE))«XC(BJ0IIE) 
♦(XC(A1)»2C(A10NE)-2C{AI)»XC(A10NE))*KC(BJ)) 

•SIM(PHI)) 

•COS(PSI) 

♦(((((-YC(AI0NE)»YC(AI))*YC(BJ0NE)-(YC(A10NE)-YC(An)*YC(BJ))»2 

♦  ((YC(A10NE)-YC(AI))*2C(8J0NE)-*(-yC(A10II£)»YC(AI))*2C(BJ))*Y 

♦  (-YC(AI0ME)-*YC(AI))*YC(BJ)*2C(BJ0ME)-»(YC(A10NE)-YC(AI))»2C(BJ)*YC(BJ0IIE) 
♦(-XC(AI)»2C(A10NE)-*2C(A1)*XC(A10NE))«XC(BJON£) 
'^(XC(AI)*2C(A10NE)-2C(A1)*XC(AIONE))*XC(BJ)) 

•COS(PHI) 

♦(((YC{AI0NE)-YC(AI))*XC(BJ0NE)*(-YC(A10N£)*YC(AI))»XC(BJ))»Z 

♦((-YC(AI0NE)*YC(AI))*2C(BJ0NE)»(YC(A10NE)-YC(AI))»2C(BJ))*X 

♦  (YC{AIONE)-YC(AI))*XC(BJ)*2C(BJOIIE) 
+(-XC(AI)*2C(AI0NE)*2C(A])*XC(A10N£))*YC(8J0IIIE) 
*(-YC(A10NE)'»YC(AI))*2C(BJ)*XC(BJ0NE) 
*(XC(AI)«2C(AIONE)-2C(AI)*XC(AIONE))»YC(BJ)) 

•SlN(PHl)*(-YC(AIONE)*YC(AI))»2C(BJO«)  +  (YC(AIONE)-YC(Al))»2C{BJ)) 

•COS (THETA) 

♦(((-YC{A10NE)*YC(AI))*XC(BJONE)*(YC(AIONE)-YC(AI))»XC(BJ))»COS(PHI) 
♦((-YC( ATONE )*YC{ AT ))*YC(BJONE)+(YC(AIONE)-YC(Al))»YC(BJ))»SIN(PHI) 
♦((YC(AIONE)-YC(AI))*XC(BJONE)»(-YC(AIONE)+YC{AI))»XC(BJ))*Y 
+((-YC(AI0N£)*YC(AI))*YC(BJ0NE)+(YC(A10NE)-VC(AI))»YC(BJ))»* 

♦  (XC(  AT )»2C( ATONE )-2C{AI)»XC(A10NE))*2C(BJ0N£) 

♦(YC( ATONE )-YC{ AT ))»XC(BJ)»YC(BJONE)*(-YC(AIONE)+YC(Al))»YC(BJ)»XC(BJOME) 
4(*XC(AT)*2C(ATONE)42C(AI)*XC(AIONEh*ZC(8J)) 

•STN(THETA) 

♦(((-XC{ATONE)+XC(AI))*XC(BJONE)+(XC(AIOHE)*XC(AT))»XC(BJ))»2 
♦((XC(AT0NE)-XC{AT))*2C(PJ0NE)+(-XC(AI0NE)+XC(AT))»2C(BJ))»X 
•{•XC( ATONE )4XC(AI))«XC(BJ)*2C(BJ0NE) 

♦  (-YC(AT)*2C(AIONE)42C(AT)*YC(AIONE))*VC(BJOIIIE) 
4(XC(AT0NE)-XC(AI))*ZC(BJ)«XC(BJ0M£) 
♦(YC(AT)«2C(AI0NE)-2C(AI)‘YC(A10HE))»YC(BJ)) 

•COS(PHl) 

♦(({-XC(AT0HE)+XC(AI))*YC(BJ0NE)4(XC(A10NE)*XC(AI))»YC(8J))»2 

4((XC(AI0NE)-XC(AI))»2C(8J0NE)4(-XC(AT0NE)*XC(AI))*2C(BJ))*Y 

♦(-XC(ATONE)*XC(AI))»YC{BJ)»ZC(BJONE)*(XC(ATONt)-XC(AI))»2C(BJ)»YC(BOONE) 

♦  (  YC(AI)»2C( ATONE )-2C(AT)»YC(AI0NE))»XC{BJ0HE) 
4(-YC(AI)«ZC(AIONE)4ZC(AI)*YC(ATONEh*XC(BJ)) 

•SIN(PHI)) 

•STN(PSI) 

♦{((-ZC(ATOH£)*ZC(AT))»XC(BJONE)*(ZC(AIONE)-ZC(AI))*XC(BJ))»COS(PHI) 

♦((-ZC(AIONE)*2C(AI))»YC(BJONE)4(ZC{AIONE)-ZC(AT))»YC(BJ))*SIN(PHI) 

♦{(ZC(ATONE)-ZC(AT))»XC(BJONE)»(-ZC(AIONE)*ZC(AT))»XC(BJ))»Y 

♦({-ZC(AT0N£)4ZC(AI))»YC(BJ0NE)4(ZC{AI0NE)-ZC(AI))»YC(BJ))»* 

♦(-XC(AI)»YC(AT0H£)+YC{AT)*XC(AT0NE))*ZC(BJ0NE) 

♦  (ZC(ATONE)-ZC(AT))»XC(BJ)*YC(BJONE)4(-ZC(ATONE)+ZC(AI))»VC{BJ)»XC(BJOI(E) 
•(XC(  AT )*YC( ATONE )-YC(AT)*XC(AI0NE})*ZC(BJ}) 

•COS(THETA) 

♦((((ZC( ATONE )-ZC(AI))*YC(flJONE)4(-ZC{AION£)*ZC(AI))*VC(BJ))»2 
♦((-ZC( ATONE )*ZC{AI ))*ZC(BJ0HE)4(2C{AI0NE)-ZC( AT ))»ZC(BJ))»y 
♦(2C(ATOHE)-ZC(AI))*YC{BJ)*2C(BJONE)4{-ZC(ATONE)4ZC(AI))»ZC(BJ)*VC(BJONE) 
♦(-XC( AT )»YC( ATONE )4YC( AT )*XC( ATONE ))»XC(BJON£) 
•(XC(AT)*YC(ATON£)-VC(AT)*XC(ATONE))*XC(BJ)} 

•COS(PHT) 

4(((-ZC(AI0HE)*ZC(AT))»XC(BJ0NE)*(ZC(AT0NE)-ZC(AT))»XC{BJ))*Z 

♦((ZC(AT0NE)-ZC(AT))«ZC(BJ0NE)+(-ZC{AI0NE)4ZC(AT))»ZC(BJ))»X 

♦(-ZC(ATONE)*ZC(AT))»XC{BJ)»ZC(BJONE) 

♦(-XC(AI)»YC(ATON£)*YC(AI)»XC(ATOHE))*YC(BJONE> 

4(ZC( ATONE )-ZC( AT ))*ZC(BJ)«XC(BJONE) 
♦(XC(AT)*YC(ATONE)*YC(AT)*XC(ATONE))»YC(BJ)) 

•STN(PHT)4(2C(  ATONE )-ZC(AT ))»ZC(BJ0NE)4(-ZC(AT0NE)*ZC( AT ))*ZC(BJ)) 
•STN(THETA)$ 

/*  H*r*r  art  tha  Applicability  conatraintt  */ 
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Type_A_*c1 •«$•[!]  : 


(((IC(U)-XC(V))»XC(l»)»COS{PHI)  +  (yC(U)-VC(V))»*C(lll)»SIIII(PHI))»COS(THtTA) 

♦  (-2C(U)»ZC(V))»XC(II)*SIN(TMETA)»(YC(U)-YC(V))»YC(II)*C0S(PMI) 
♦(-XC(U)*XC(V))»YC{M)»SIN{PH1)) 

•COS(PSI) 

+  (((-XC(U)*XC(V))»YC(M)*COS(PHl)*(-YC(U)»YC{V))*YC(ll)»SlN{PHI))»COS(  THETA) 

♦  (2C(U)-2C(V))»YC(N)*SIN{1HETA)  +  {YC(U)-YC(V))»XC(II)*C0S(PHI) 
♦(-XC(U)*XC(V))*XC(N)»SIN(PH1)) 

•SI»(PSl)*(2C(U)-ZC(V))»ZC(N)»COS(THETA) 

♦  ((XC(U)-XC(V))*2C(N)*COS(PHl)«(YC(U)-YC(V))»2C(»l)»SI»l(PHI))*SIN(THETA)-LIVEL$ 


Type_B_AcTause[l]  ; 


(((-XC(N)»XC(V)-XC(N)*XC(U))»COS(PHl)-*(-YC{ll)*XC(V)*YC(ll)»XC{U))*SlN(PHI)) 

•COS(THETA) 

+(ZC(N)*XC(V)-2C(N)»XC(U))*S1N(THETA)*(-YC{H)*YC(V)*YC(M)*YC(U))»C0S(PHI) 

♦(XC(N)»YC(V)-XC(II)*VC{U))*S1H(PHI)) 

•COS(PSl) 

♦  (({XC(N)»YC(V)-XC(N)*YC(U))*COS(PHI)+(YC(ll)*YC(V)-YC(M)»YC(U))*SIN(PHI)) 
•COS(THETA) 

♦(-ZC(N)*YC(V)+ZC(H)»YC{U))‘S1N(THETA)*{-YC(H)*XC(V)+YC(H)*XC(U))»C0S(PH1) 
♦  (XC(»I)*XC(V)-XC(N)»XC(U))*SIN(PHI)) 
•SIN(PSI)-(-ZC{N)»ZC(V)+2C(N)*2C(U))«COS(THETA) 

♦  ((-XC(N)*ZC(V)*XC{N)*2C(U))*COS(PHI)  +  (-YC(ll)*2C(W)+YC(l()*ZC(U))*SIII(PHl)) 
•SIN(THETA)-LEVELS 


/•  mode:  macsyma  *•- 

(Bruce  Donald.  Here  ae  [xpress  a  Constraint  (C-surfaco  or 
Acf)  In  Canonical  Linear  rorn  and  Canonical  Trig  form. 

Given  a  CONSTRAINT  vMch  Is  either  a 

C-surface  or  an  ACT  (applicability  clausa  function) 

and  a  variable  (VAR)  ae  solve  for  the  variable)  */ 

Load_up():«  /•  load  necessary  files  •/ 

b1ock([],  /•  for  solving  •/ 

If  ALL_DErS_LOADED  -  TRUE  then  "OIC* 

else  batch1oad(''usrd$:[brd. prodjdef abc .mac*) , 
batchToad([intabc,mac]). 
batch1oad(  "usrdS ; [brd . prod ]ana1yre2 .mac") . 

RalVars:  [i,  y,  x. 

sin(phi),  cos(phi),  t1n(  theta),  cos(theta), 
sin(psi).  cos(psi) 

Angles:  [Phi,  Psi,  theta])$ 

Load_up()S 

/•  give  us  an  “explicit*  tangent  space  •/ 

s(var)  :■  tin(var)$ 
c(var)  :■  cos(var)$ 

Bu11d_Man1fold()$  /•  Rebuild  Manifold  •/ 

solve_for_angle(exp.var):»  /•  Solve  for  COS(var)  •/ 

block([Rats.  Rl.  R2,  R3]. 
rats:  ratvars, 

Ratvars:  [c(var),  s(var)], 
print(’Sinpllfying. . .*), 

Rl:  Iso1ateN(exp,  Ratvars), 

Printed  iminating  *,  s(var),*...*), 

R2:  d iniinate[l](Rl,  Var), 

Ldisp(R2), 

Print(’So1ving  for  *,  c(var),*,.,*), 

R3:  tolve(R2 ,c(var)) , 
display_and_gr1nd(R3) , 
ratvart:ratt, 
r3)S 

Solve_for_K(Exp,  Kvar):«  /•  Solve  for  Any  Var  •/ 

Block([RBts.  rl,  R2], 

Rats:  ratvars, 

Ratvars:  [Xvar], 
print ("Sirapl ifying, . .“), 

Rl:  Iso1ateN(Exp,  Ratvars). 
pr1nt(’  Solving  for  *.  *var.’...“), 
r2:  solve(Rl,  Xvar), 

0isplay_and_grind(r2) . 
ratvars:  rats, 

R2)S 

Solve_test():-  /•  Test  the  Solution  Routines  •/ 

t1ock([]. 

grind(soive_test). 

grind:false, 

Solve_for_angle(Csl.  Phi), 

Solve_for_*(C51.  X). 

/•  solve  a  type  (a)  surface  for  PSI  and  T  •/ 

ASl:  SlnCos_to_CS(type_A_CSurface[13). 

Solve_for_angle(ASl ,  Psi), 

Solve_for_K(ASl.  y). 


/•  solve  a  type  (B)  surface  for  PSI  and  V  •/ 


BSl ;  SinCos_to_CS( type_B_CSurf •€•[!]) . 


So1vt_for_ang1t(BSl ,  Bti), 

Solvt_for_X(BSl.  y). 

BotifyO 

)S 

/*  CExpress  eipresses  things  Canonically  */ 

CEiiprass(Ejip,  Type)  :• 

BLock([3, 

If  Type  •  C_Surfece  then 
(PrintCCanonical  Linear  form 

Canonica1_1 inear_f orn( E»p) ) , 

Pr int( "Sol ving  for  Angles...*), 
for  Var  in  Angles  do 

(Solve_f or_angl g(Exp ,  Var)))S 

/*  Here  «e  Do  the  Expression.  Nom  to  get  Ground  forms,  just  change  GRIND,  etc.  */ 

DO_CExprets():« 

BLocli([]. 

Grind:  False.  /•  Here  ft  fs..  •/ 

kill(labe'lt), 

CExpress( type_A_Csurface[l],  *C_Surfac#) , 

CExpress( type_B_Csurfaee[l] .  'C.Surfaca). 

CExpressj type_C_Csurf aee[l].  'C.Surface). 

CExpress( type_A_Aclause[l].  ’ACF). 

CExpress( type_B_AClause[l],  *ACF))S 


h\ 


/•  Mactyma  BRD90Z  (Bruce  R.  Donald). 

Sove  intarsactions  of  C.surfacas  and  A.clausaa. 
for  tha  6dof  novars  problam.  ■/ 

Angles:  [thata.  Phi,  Psi]S 


/•  a11o«  some  simplification  into  C  and  S  terms  •/ 

SinCoS_tO_CS  (a»p)  ;• 

B1ock([t]. 

C:  axp, 

for  Var  in  Angies  Do 
C:  ratsubst(s[var],  sin(var), 

ratsubst(c[vari.  cos(var),  E)). 

rat(E))$ 

/•  permit  the  inverse  •/ 

CS_to_SinCos  (axp)  :• 

B1ock([E3. 

E:  exp, 

for  Var  in  Angles  Do 
E:  ratsubst(sin(var),  stvar], 

ratsubst( cos( var) ,c[var3.  E)). 

£)8 

/*  The  Tangent  Space  Manifold  */ 

/*  00  it  yojrsaif,  pail  batchload([defabc,mac.  'usrdS*.  brd])  */ 

/•  Short  form  functions  •/ 

t(var)  :•  t[var]$ 
c(var)  :•  cfvarjs 

/•  Simplify  if  poss>oleT  use  the  i/eCvar]  form  though.  •/ 

Simp_l(Exp.  Var)  ;•  ratsut;.t(l,  t(var)*2  ♦  c(var)‘2,  oxp)$ 
timp_3(Exp)  :■  Rat( 

Simp_l(  SiRp_l(  Simp_l(Exp.  Phi),  Theta).  Pa1))S 

manifold(var)  :•  s(var)‘2  ♦  c(var)‘‘2  •  IS 

Bu11d_manifo1d()  :■ 

Biock([]. 

Man[thcta3:  nanifold(thata), 

ManfPhi]  :  Manifold(phl), 

Man[Pti]  ;  Manifoid(pti))$ 

Bui1d_manifo1d()S 

/■  Eiiminata[l]  eliminates  the  dual  trig  term.  S‘l  is  paraneterixad  by 
one  variable  (var).  Eliminates  SIN(VAR}  from  EXP  */ 

a1 iminate[l](exp,  var)  :■ 
b1ock(n. 

Tempi:  rat(part( 

ai iminata([exp,  Man[var3].  [s(var)]),l)) 

)8 

/*  Eliminate[2]  calls  eliminate[l]  twice,  and  eliminates  the  resultants. 
Hence  the  intersection  of  two  level  surfaces  on  S*3  Is 
parameterized  by  a  one  param.  family.  Eliminates  Sin/cos  VarlfVar2 
from  Exp.  •/ 

US£_CS_F0RM;  TrueS 

el iminate[2](expl ,  exp2,  varl,  var2)  ;■ 


/•  T«mp  retuH*  •/ 
/•  Titeir  Labels  •/ 


b1ock(  [Rl.  R2.  R3,  R4.  R5.  r6. 

Lrl.  LrZ,  Lr3.  Lr4.  trS.  Ir6]. 

RatVars;  [a.  y.  t, 

s(phi).  c(phi).  i(  theta),  c(theta). 
s(psi).  c(p$i)  ]. 
if  Use_CS_FORM  then 

(e»pl:  SinCoi_IO_CS(e*pl ) . 
e«p2:  SinCos_To_CS(e»p2) ) . 
printC'Eliminate  “.s(varl).'  from  eapression  1:"). 
rl:  el ininate[l](e»pl ,  »arl), 

Lrl;l()isp(rl). 

printCEl  inmate  ■,s(varl),’  fron  eapression  2:"). 
r2:  el ininate[ 1 ]( e»p2 ,  varl), 

Ir?: 10isp(r2). 

printC'Eliminate  ■,c(varl).-  from  “ . append(LRl .  LR2)). 
r3:  rat(elininate{[rl,r2].[c(varl)])). 

Lr3;  Ldisp(R3), 

pr int ( "E 1  ini nate  ■.s(war2).*  froB  '.LrS). 

R4,'  el  ininate[l](R3,  var2), 

Lr4;  Laisp(R4), 

PrintC  Solve  ”,  1r4.“  for  ”,  c{var2)). 

R5:  rat(Solve(R4.  c(var2))). 

LfS:  Ldisp(R5). 

printCEinally.  solve  ”.Lrl.'  for  '.c(varl)). 

R6:  rat(solve(Rl.  c(varl))). 

Lr6;  ldisp(R6), 

Append{lr2.1r2,lr3.1r4.1r5.1r6))$ 


Test{ ) :• 

el  iniinate[2](type_A_Aclause[l3. 

type_A_Aclause[2], 


/•  node:  nacsyna  *•* 

(file  to  Run  Production  of  3d  Space  equations. 
Bruce  R.  Donald.  MIT  AI  LAB  )  •/ 

/•  Define  the  type  ABC  constraints.  •/ 

Def ine_ABC( )  :• 
b1ock([]. 

l)atch(  "USRDS;  [brd .  prod]Cspace .  mac'))S 

/•  Define  the  appH  icafiil  ity  Constraints  •/ 

Def ine_App1 ic( )  :• 

B1ock([]. 

BatchCUSRDS:  [brd.prod]App1  ic.mac*))$ 


/•  Define  Both  •/ 


Produce_Oef  s( ) : • 
b1ock([]. 

Writef  ne("usrd$:rbrd.prod]ProOuce.10B"). 

Batch("usrd$:[brd.prod]Rotate.mac*). 

Define_ABC(). 

Def ine.AppI ic( ) , 

C1osefi1s(), 

«lotify())S 

Co1d_Rettart()  :• 
b1ock{[]. 

batchload("iys$1oBin;utns.inac“). 

batchload(’usrdS;[brd.proO]produce.mac*). 

Produce_defs())$ 

/•  Here's  a  function  to  save  labels  for  you.  •/ 

Save_labe1s(fiTe):* 
b1ock([] . 

?Open_output_fiT#(f lie). 

for  Label  in  Appsnd(  reverse{laba1s(B)).  raverse(labeis(d))) 

DO 

(rGr1nd_T0_fILE(L8del).  Prlnt(Labal)). 

7C1o$e_0utput_f11a())$ 

/•  Produce  the  EXPRESS  tile,  parsing  Into  solutions  and  coefficients.  •/ 

Produce_E*press():* 

block([], 

■ritaf ile(’usrd$;[brd.prod]E*press.Tog*). 
batch ("usrdS: [brd. prod]E»pres$ .mac"). 

DO_CE*press(), 

PrintC  Saving  Labels  In  ISP  ffJe...'). 

Save  Label s (-USRDS ; [brd. prod]EXPRESS.LSP'). 

"otify())$ 

/•  this  function  produces  EVERYTHING.  •/ 

Produce_ALL( ) :■ 

8L0CX([]. 

Produce_Oefs(). 

ALL_DEFS_L0A0ED:  TRUE. 

KnT(Nfi.  Ngj.  Ai.  Alone.  BJ.  BJona). 

Type_A_Csurface[l]  :  A_6. 

Type_B_Csurf8ce[li:  B_S, 

Type_C_Csur f ace[ 1] :  C_6. 

Produce_e«pre$s{). 

elosefi1e())$ 


256 


/•  Bruce  R.  Donald  {BRD902)  tuler  Rotations  for  Macsyma.  See  MACSYMA  -• 
Paul,  p  45  •/ 

c(angle):*  cos(Bng1e)$ 
s(angle):*  sin(angle)$ 

Rot_*_psi: 

Matr1i( 

[  c{psi).  -s(psi).  0.  0]. 

[  s(pii).  e{psi).  0.  0], 

[  0.  0.  1.  0]. 

[  0.  0.  0.  1]); 

Rot_y_thet8:  raatri»{ 

[  c(theta).  0.  s(theta),  0], 
t  0.  1.  0.  0]. 

[-s(tl)etB),  0.  c(lheta).  0], 

[  0.  0,  0.  1]): 

Rot_Z_Ph1:  Katr1i( 

[  c(phi).  -s(phi).  0.  0], 

[  s(ph1).  c(phi).  0.  0], 

C  0.  0.  1,  0], 

[  0.  0.  0.  1]); 

Eul#r_tenp:  Rot_V_Theta  .  Rot_2_P$1; 

Eu1ar_niatrii;  Rot_Z_Ph1  .  Eular.tamp: 

homoganize(X):«  [>[1],  «C2],  x[3].  1]: 

UnHomogen1ze(X]  :•  [*Cl]/*C4].  *[33'*C*]3i 

Rotate_vector(X]  :■  UnHomogen1ze(  Eu1er_Matr1»  .  Hcncgentze(X))  ; 

/•  Cross  Product  •/ 

Cross(A,B)  :■ 

[  (sC2]«b[3]  •  •C3]‘6[2]). 

(aC3]*b[l]  -  a[l]*6[3]). 

(•[ll‘b[2]  -  »[2]«b[l])]; 

/•  Simplify  if  possIbleT  •/ 

S1mp«l(Esp.  Var)  ;•  ratsubst(l.  s1n(var)*2  ♦  eos(var)*2.  exp); 
s1mp_3(Exp)  :■  Rat( 

Slmp_l(  S1mp_l(  Slmp_l(Exp,  Pbl],  Theta).  Pal)); 

/*  General  Transformation  function.  A  4>vector  Is  assumed  to  be  a  plant, 
and  a  3-vector  a  3-vector,  hence  the  3-rector  Is  ROTATED  and  a  4-vector 
plana  Is  also  rotated...  See  PAUL  */ 

Transform(Transformat1on_matr1x,  X)  ;■ 

B1ock([Kom.  Trans. ans]. 

Ans:  'WhoopsI*. 

If  length(X)  «  4  then 

Ans:  X  .Transformat1on_natr1x 
Else 

If  length  (x)  ■  3  then 

Ans:Unhomogen1zs(Transformat1on_matr1x  .  Homogenize  (X)) 

Else  Prlnt(”  But  “,x."  Mutt  be  a  3-vector  or  4-Vectorl"), 

Ans )  ; 


/*  Boa  Compute  Inverse  of  the  Euler  Transformation  */ 
E_Adj:  s1mp_3(rat  (adJolnt(  tuler_matr1x))) ; 


« 
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